<commit_before>/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2006 Artem Pavlenko
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library 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; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

//$Id: postgisfs.cc 34 2005-04-04 13:27:23Z pavlenko $

#include <boost/algorithm/string.hpp>
#include <mapnik/global.hpp>
#include <mapnik/wkb.hpp>
#include <mapnik/unicode.hpp>
#include "postgis.hpp"

using boost::lexical_cast;
using boost::bad_lexical_cast;
using boost::trim;
using std::string;

postgis_featureset::postgis_featureset(boost::shared_ptr<ResultSet> const& rs,
                                       std::string const& encoding,
                                       unsigned num_attrs=0)
    : rs_(rs),
      num_attrs_(num_attrs),
      totalGeomSize_(0),
      tr_(new transcoder(encoding)),
      count_(0)  {}

feature_ptr postgis_featureset::next()
{
    if (rs_->next())
    {
        feature_ptr feature(new Feature(count_));
        int size=rs_->getFieldLength(0);
        const char *data=rs_->getValue(0);
        geometry_ptr geom=geometry_utils::from_wkb(data,size,-1);
        totalGeomSize_+=size;

        if (geom)
        {
            feature->set_geometry(geom);

            for (unsigned pos=1;pos<num_attrs_+1;++pos)
            {
                std::string name = rs_->getFieldName(pos);
                const char* buf=rs_->getValue(pos);
                int oid = rs_->getTypeOID(pos);

                if (oid==23) //int4
                {
                    int val = int4net(buf);
                    boost::put(*feature,name,val);
                }
                else if (oid==21) //int2
                {
                    int val = int2net(buf);
                    boost::put(*feature,name,val);
                }
                else if (oid == 700) // float4
                {
                    float val;
                    float4net(val,buf);
                    boost::put(*feature,name,val);
                }
                else if (oid == 701) // float8
                {
                    double val;
                    float8net(val,buf);
                    boost::put(*feature,name,val);
                }
                else if (oid==25 || oid==1042 || oid==1043) // text or bpchar or varchar
                {
                    std::string str(buf);
                    trim(str);
                    std::wstring wstr = tr_->transcode(str);
                    boost::put(*feature,name,wstr);
                }
                else
                {
#ifdef MAPNIK_DEBUG
                    std::clog << "uknown OID = " << oid << " FIXME \n";
#endif
                    //boost::put(*feature,name,0);
                }
            }
            ++count_;
        }
        return feature;
    }
    else
    {
        rs_->close();
        return feature_ptr();
    }
}

postgis_featureset::~postgis_featureset()
{
    rs_->close();
}
<commit_msg>corrected members init order<commit_after>/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2006 Artem Pavlenko
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library 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; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

//$Id: postgisfs.cc 34 2005-04-04 13:27:23Z pavlenko $

#include <boost/algorithm/string.hpp>
#include <mapnik/global.hpp>
#include <mapnik/wkb.hpp>
#include <mapnik/unicode.hpp>
#include "postgis.hpp"

using boost::lexical_cast;
using boost::bad_lexical_cast;
using boost::trim;
using std::string;

postgis_featureset::postgis_featureset(boost::shared_ptr<ResultSet> const& rs,
                                       std::string const& encoding,
                                       unsigned num_attrs=0)
    : rs_(rs),
      num_attrs_(num_attrs),
      tr_(new transcoder(encoding)),
      totalGeomSize_(0),
      count_(0)  {}

feature_ptr postgis_featureset::next()
{
    if (rs_->next())
    {
        feature_ptr feature(new Feature(count_));
        int size=rs_->getFieldLength(0);
        const char *data=rs_->getValue(0);
        geometry_ptr geom=geometry_utils::from_wkb(data,size,-1);
        totalGeomSize_+=size;

        if (geom)
        {
            feature->set_geometry(geom);

            for (unsigned pos=1;pos<num_attrs_+1;++pos)
            {
                std::string name = rs_->getFieldName(pos);
                const char* buf=rs_->getValue(pos);
                int oid = rs_->getTypeOID(pos);

                if (oid==23) //int4
                {
                    int val = int4net(buf);
                    boost::put(*feature,name,val);
                }
                else if (oid==21) //int2
                {
                    int val = int2net(buf);
                    boost::put(*feature,name,val);
                }
                else if (oid == 700) // float4
                {
                    float val;
                    float4net(val,buf);
                    boost::put(*feature,name,val);
                }
                else if (oid == 701) // float8
                {
                    double val;
                    float8net(val,buf);
                    boost::put(*feature,name,val);
                }
                else if (oid==25 || oid==1042 || oid==1043) // text or bpchar or varchar
                {
                    std::string str(buf);
                    trim(str);
                    std::wstring wstr = tr_->transcode(str);
                    boost::put(*feature,name,wstr);
                }
                else
                {
#ifdef MAPNIK_DEBUG
                    std::clog << "uknown OID = " << oid << " FIXME \n";
#endif
                    //boost::put(*feature,name,0);
                }
            }
            ++count_;
        }
        return feature;
    }
    else
    {
        rs_->close();
        return feature_ptr();
    }
}

postgis_featureset::~postgis_featureset()
{
    rs_->close();
}
<|endoftext|>


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

/* This file declares stripped-down versions of functions that
 * normally exist outside of the glsl folder, so that they can be used
 * when running the GLSL compiler standalone (for unit testing or
 * compiling builtins).
 */

#include "standalone_scaffolding.h"

#include <assert.h>
#include <string.h>
#include <limits.h>
#include "util/ralloc.h"

void
_mesa_reference_shader(struct gl_context *ctx, struct gl_shader **ptr,
                       struct gl_shader *sh)
{
   (void) ctx;
   *ptr = sh;
}

void
_mesa_shader_debug(struct gl_context *, GLenum, GLuint *id,
                   const char *, int)
{
}

extern "C" void
_mesa_error_no_memory(const char *caller)
{
}

struct gl_shader *
_mesa_new_shader(struct gl_context *ctx, GLuint name, GLenum type)
{
   struct gl_shader *shader;

   (void) ctx;

   assert(type == GL_FRAGMENT_SHADER || type == GL_VERTEX_SHADER);
   shader = rzalloc(NULL, struct gl_shader);
   if (shader) {
      shader->Type = type;
      shader->Stage = _mesa_shader_enum_to_shader_stage(type);
      shader->Name = name;
      shader->RefCount = 1;
   }
   return shader;
}

void initialize_context_to_defaults(struct gl_context *ctx, gl_api api)
{
   memset(ctx, 0, sizeof(*ctx));

   ctx->API = api;

   ctx->Extensions.dummy_false = false;
   ctx->Extensions.dummy_true = true;
   ctx->Extensions.ARB_compute_shader = true;
   ctx->Extensions.ARB_conservative_depth = true;
   ctx->Extensions.ARB_draw_instanced = true;
   ctx->Extensions.ARB_ES2_compatibility = true;
   ctx->Extensions.ARB_ES3_compatibility = true;
   ctx->Extensions.ARB_explicit_attrib_location = true;
   ctx->Extensions.ARB_fragment_coord_conventions = true;
   ctx->Extensions.ARB_fragment_layer_viewport = true;
   ctx->Extensions.ARB_gpu_shader5 = true;
   ctx->Extensions.ARB_sample_shading = true;
   ctx->Extensions.ARB_shader_bit_encoding = true;
   ctx->Extensions.ARB_shader_stencil_export = true;
   ctx->Extensions.ARB_shader_texture_lod = true;
   ctx->Extensions.ARB_shading_language_420pack = true;
   ctx->Extensions.ARB_shading_language_packing = true;
   ctx->Extensions.ARB_texture_cube_map_array = true;
   ctx->Extensions.ARB_texture_gather = true;
   ctx->Extensions.ARB_texture_multisample = true;
   ctx->Extensions.ARB_texture_query_levels = true;
   ctx->Extensions.ARB_texture_query_lod = true;
   ctx->Extensions.ARB_uniform_buffer_object = true;
   ctx->Extensions.ARB_viewport_array = true;

   ctx->Extensions.OES_EGL_image_external = true;
   ctx->Extensions.OES_standard_derivatives = true;

   ctx->Extensions.EXT_shader_integer_mix = true;
   ctx->Extensions.EXT_texture3D = true;
   ctx->Extensions.EXT_texture_array = true;
   ctx->Extensions.EXT_draw_buffers = true;

   ctx->Extensions.NV_texture_rectangle = true;

   ctx->Const.GLSLVersion = 120;

   /* 1.20 minimums. */
   ctx->Const.MaxLights = 8;
   ctx->Const.MaxClipPlanes = 6;
   ctx->Const.MaxTextureUnits = 2;
   ctx->Const.MaxTextureCoordUnits = 2;
   ctx->Const.Program[MESA_SHADER_VERTEX].MaxAttribs = 16;

   ctx->Const.Program[MESA_SHADER_VERTEX].MaxUniformComponents = 512;
   ctx->Const.Program[MESA_SHADER_VERTEX].MaxOutputComponents = 32;
   ctx->Const.MaxVarying = 8; /* == gl_MaxVaryingFloats / 4 */
   ctx->Const.Program[MESA_SHADER_VERTEX].MaxTextureImageUnits = 0;
   ctx->Const.MaxCombinedTextureImageUnits = 2;
   ctx->Const.Program[MESA_SHADER_FRAGMENT].MaxTextureImageUnits = 2;
   ctx->Const.Program[MESA_SHADER_FRAGMENT].MaxUniformComponents = 64;
   ctx->Const.Program[MESA_SHADER_FRAGMENT].MaxInputComponents = 32;

   ctx->Const.MaxDrawBuffers = 1;
   ctx->Const.MaxComputeWorkGroupCount[0] = 65535;
   ctx->Const.MaxComputeWorkGroupCount[1] = 65535;
   ctx->Const.MaxComputeWorkGroupCount[2] = 65535;
   ctx->Const.MaxComputeWorkGroupSize[0] = 1024;
   ctx->Const.MaxComputeWorkGroupSize[1] = 1024;
   ctx->Const.MaxComputeWorkGroupSize[2] = 64;
   ctx->Const.MaxComputeWorkGroupInvocations = 1024;
   ctx->Const.Program[MESA_SHADER_COMPUTE].MaxTextureImageUnits = 16;
   ctx->Const.Program[MESA_SHADER_COMPUTE].MaxUniformComponents = 1024;
   ctx->Const.Program[MESA_SHADER_COMPUTE].MaxInputComponents = 0; /* not used */
   ctx->Const.Program[MESA_SHADER_COMPUTE].MaxOutputComponents = 0; /* not used */

   /* Set up default shader compiler options. */
   struct gl_shader_compiler_options options;
   memset(&options, 0, sizeof(options));
   options.MaxUnrollIterations = 32;
   options.MaxIfDepth = UINT_MAX;

   /* Default pragma settings */
   options.DefaultPragmas.Optimize = true;

   for (int sh = 0; sh < MESA_SHADER_STAGES; ++sh)
      memcpy(&ctx->Const.ShaderCompilerOptions[sh], &options, sizeof(options));
}
<commit_msg>postmerge: temporarily revert max unroll iterations to previous default value (8)<commit_after>/*
 * Copyright © 2011 Intel Corporation
 *
 * Permission is hereby granted, free of charge, to any person obtaining a
 * copy of this software and associated documentation files (the "Software"),
 * to deal in the Software without restriction, including without limitation
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
 * and/or sell copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice (including the next
 * paragraph) shall be included in all copies or substantial portions of the
 * Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
 * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
 * DEALINGS IN THE SOFTWARE.
 */

/* This file declares stripped-down versions of functions that
 * normally exist outside of the glsl folder, so that they can be used
 * when running the GLSL compiler standalone (for unit testing or
 * compiling builtins).
 */

#include "standalone_scaffolding.h"

#include <assert.h>
#include <string.h>
#include <limits.h>
#include "util/ralloc.h"

void
_mesa_reference_shader(struct gl_context *ctx, struct gl_shader **ptr,
                       struct gl_shader *sh)
{
   (void) ctx;
   *ptr = sh;
}

void
_mesa_shader_debug(struct gl_context *, GLenum, GLuint *id,
                   const char *, int)
{
}

extern "C" void
_mesa_error_no_memory(const char *caller)
{
}

struct gl_shader *
_mesa_new_shader(struct gl_context *ctx, GLuint name, GLenum type)
{
   struct gl_shader *shader;

   (void) ctx;

   assert(type == GL_FRAGMENT_SHADER || type == GL_VERTEX_SHADER);
   shader = rzalloc(NULL, struct gl_shader);
   if (shader) {
      shader->Type = type;
      shader->Stage = _mesa_shader_enum_to_shader_stage(type);
      shader->Name = name;
      shader->RefCount = 1;
   }
   return shader;
}

void initialize_context_to_defaults(struct gl_context *ctx, gl_api api)
{
   memset(ctx, 0, sizeof(*ctx));

   ctx->API = api;

   ctx->Extensions.dummy_false = false;
   ctx->Extensions.dummy_true = true;
   ctx->Extensions.ARB_compute_shader = true;
   ctx->Extensions.ARB_conservative_depth = true;
   ctx->Extensions.ARB_draw_instanced = true;
   ctx->Extensions.ARB_ES2_compatibility = true;
   ctx->Extensions.ARB_ES3_compatibility = true;
   ctx->Extensions.ARB_explicit_attrib_location = true;
   ctx->Extensions.ARB_fragment_coord_conventions = true;
   ctx->Extensions.ARB_fragment_layer_viewport = true;
   ctx->Extensions.ARB_gpu_shader5 = true;
   ctx->Extensions.ARB_sample_shading = true;
   ctx->Extensions.ARB_shader_bit_encoding = true;
   ctx->Extensions.ARB_shader_stencil_export = true;
   ctx->Extensions.ARB_shader_texture_lod = true;
   ctx->Extensions.ARB_shading_language_420pack = true;
   ctx->Extensions.ARB_shading_language_packing = true;
   ctx->Extensions.ARB_texture_cube_map_array = true;
   ctx->Extensions.ARB_texture_gather = true;
   ctx->Extensions.ARB_texture_multisample = true;
   ctx->Extensions.ARB_texture_query_levels = true;
   ctx->Extensions.ARB_texture_query_lod = true;
   ctx->Extensions.ARB_uniform_buffer_object = true;
   ctx->Extensions.ARB_viewport_array = true;

   ctx->Extensions.OES_EGL_image_external = true;
   ctx->Extensions.OES_standard_derivatives = true;

   ctx->Extensions.EXT_shader_integer_mix = true;
   ctx->Extensions.EXT_texture3D = true;
   ctx->Extensions.EXT_texture_array = true;
   ctx->Extensions.EXT_draw_buffers = true;

   ctx->Extensions.NV_texture_rectangle = true;

   ctx->Const.GLSLVersion = 120;

   /* 1.20 minimums. */
   ctx->Const.MaxLights = 8;
   ctx->Const.MaxClipPlanes = 6;
   ctx->Const.MaxTextureUnits = 2;
   ctx->Const.MaxTextureCoordUnits = 2;
   ctx->Const.Program[MESA_SHADER_VERTEX].MaxAttribs = 16;

   ctx->Const.Program[MESA_SHADER_VERTEX].MaxUniformComponents = 512;
   ctx->Const.Program[MESA_SHADER_VERTEX].MaxOutputComponents = 32;
   ctx->Const.MaxVarying = 8; /* == gl_MaxVaryingFloats / 4 */
   ctx->Const.Program[MESA_SHADER_VERTEX].MaxTextureImageUnits = 0;
   ctx->Const.MaxCombinedTextureImageUnits = 2;
   ctx->Const.Program[MESA_SHADER_FRAGMENT].MaxTextureImageUnits = 2;
   ctx->Const.Program[MESA_SHADER_FRAGMENT].MaxUniformComponents = 64;
   ctx->Const.Program[MESA_SHADER_FRAGMENT].MaxInputComponents = 32;

   ctx->Const.MaxDrawBuffers = 1;
   ctx->Const.MaxComputeWorkGroupCount[0] = 65535;
   ctx->Const.MaxComputeWorkGroupCount[1] = 65535;
   ctx->Const.MaxComputeWorkGroupCount[2] = 65535;
   ctx->Const.MaxComputeWorkGroupSize[0] = 1024;
   ctx->Const.MaxComputeWorkGroupSize[1] = 1024;
   ctx->Const.MaxComputeWorkGroupSize[2] = 64;
   ctx->Const.MaxComputeWorkGroupInvocations = 1024;
   ctx->Const.Program[MESA_SHADER_COMPUTE].MaxTextureImageUnits = 16;
   ctx->Const.Program[MESA_SHADER_COMPUTE].MaxUniformComponents = 1024;
   ctx->Const.Program[MESA_SHADER_COMPUTE].MaxInputComponents = 0; /* not used */
   ctx->Const.Program[MESA_SHADER_COMPUTE].MaxOutputComponents = 0; /* not used */

   /* Set up default shader compiler options. */
   struct gl_shader_compiler_options options;
   memset(&options, 0, sizeof(options));
   options.MaxUnrollIterations = 8;
   options.MaxIfDepth = UINT_MAX;

   /* Default pragma settings */
   options.DefaultPragmas.Optimize = true;

   for (int sh = 0; sh < MESA_SHADER_STAGES; ++sh)
      memcpy(&ctx->Const.ShaderCompilerOptions[sh], &options, sizeof(options));
}
<|endoftext|>


<commit_before>//
// Copyright (c) 2019, Wolfgang Merkt
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//  * Redistributions of source code must retain the above copyright notice,
//    this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of  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.
//

#include <exotica_quadrotor_dynamics_solver/quadrotor_dynamics_solver.h>

REGISTER_DYNAMICS_SOLVER_TYPE("QuadrotorDynamicsSolver", exotica::QuadrotorDynamicsSolver)

namespace exotica
{
QuadrotorDynamicsSolver::QuadrotorDynamicsSolver()
{
    num_positions_ = 6;
    num_velocities_ = 6;
    num_controls_ = 4;

    J_.setZero();
    J_.diagonal() = Eigen::Vector3d(0.0023, 0.0023, 0.004);

    J_inv_.setZero();
    J_inv_.diagonal() = Eigen::Vector3d(1. / 0.0023, 1. / 0.0023, 1. / 0.0040);
}

void QuadrotorDynamicsSolver::AssignScene(ScenePtr scene_in)
{
    const int num_positions_in = scene_in->GetKinematicTree().GetNumControlledJoints();
    // TODO: This is a terrible check (not against name etc.), but can stop _some_ mismatches between URDF/model and dynamics
    if ((num_positions_in) != 6 || scene_in->GetKinematicTree().GetControlledBaseType() != BaseType::FLOATING)
        ThrowPretty("Robot model may not be a quadrotor.");
}

Eigen::VectorXd QuadrotorDynamicsSolver::f(const StateVector& x, const ControlVector& u)
{
    const Eigen::Vector3d& translation = x.topRows<3>();
    const Eigen::Vector3d& rpy = x.middleRows<3>(3);
    const Eigen::Vector3d& v = x.middleRows<3>(6);
    const Eigen::Vector3d& omega = x.bottomRows<3>();

    // Create quaternion from rpy
    const Eigen::Quaterniond quaternion = Eigen::AngleAxisd(rpy(0), Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(rpy(1), Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(rpy(2), Eigen::Vector3d::UnitZ());

    const double F_1 = k_f_ * u(0);
    const double F_2 = k_f_ * u(1);
    const double F_3 = k_f_ * u(2);
    const double F_4 = k_f_ * u(3);
    const Eigen::Vector3d F(0, 0, F_1 + F_2 + F_3 + F_4);  // total rotor force in body frame

    const double M_1 = k_m_ * u(0);
    const double M_2 = k_m_ * u(1);
    const double M_3 = k_m_ * u(2);
    const double M_4 = k_m_ * u(3);
    const Eigen::Vector3d tau(L_ * (F_2 - F_4), L_ * (F_3 - F_1), (M_1 - M_2 + M_3 - M_4));  // total rotor torque in body frame

    const Eigen::Quaterniond dquaternion = (quaternion * Eigen::Quaterniond(0, omega(0), omega(1), omega(2)));
    const Eigen::Vector3d rpy_dot = dquaternion.toRotationMatrix().eulerAngles(0, 1, 2);

    StateVector x_dot;
    x_dot.topRows<3>() = v;                                                                 // velocity in world frame
    x_dot.middleRows<3>(3) = rpy_dot;                                                       // via quaternion derivative
    x_dot.middleRows<3>(6) = Eigen::Vector3d(0, 0, -g_) + (1. / mass_) * (quaternion * F);  // acceleration in world frame
    x_dot.middleRows<3>(9) = J_inv_ * (tau - omega.cross(J_ * omega));                      // Euler's equation : I* ω + ω x I* ω = constraint_decrease_ratio

    return x_dot;
}

Eigen::MatrixXd QuadrotorDynamicsSolver::fx(const StateVector& x, const ControlVector& u)
{
    // TODO
}

Eigen::MatrixXd QuadrotorDynamicsSolver::fu(const StateVector& x, const ControlVector& u)
{
    // TODO
}

}  // namespace exotica
<commit_msg>[exotica_quadrotor_dynamics_solver] Fix RPY example<commit_after>//
// Copyright (c) 2019, Wolfgang Merkt
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//  * Redistributions of source code must retain the above copyright notice,
//    this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of  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.
//

#include <exotica_quadrotor_dynamics_solver/quadrotor_dynamics_solver.h>

REGISTER_DYNAMICS_SOLVER_TYPE("QuadrotorDynamicsSolver", exotica::QuadrotorDynamicsSolver)

namespace exotica
{
QuadrotorDynamicsSolver::QuadrotorDynamicsSolver()
{
    num_positions_ = 6;
    num_velocities_ = 6;
    num_controls_ = 4;

    J_.setZero();
    J_.diagonal() = Eigen::Vector3d(0.0023, 0.0023, 0.004);

    J_inv_.setZero();
    J_inv_.diagonal() = Eigen::Vector3d(1. / 0.0023, 1. / 0.0023, 1. / 0.0040);
}

void QuadrotorDynamicsSolver::AssignScene(ScenePtr scene_in)
{
    const int num_positions_in = scene_in->GetKinematicTree().GetNumControlledJoints();
    // TODO: This is a terrible check (not against name etc.), but can stop _some_ mismatches between URDF/model and dynamics
    if ((num_positions_in) != 6 || scene_in->GetKinematicTree().GetControlledBaseType() != BaseType::FLOATING)
        ThrowPretty("Robot model may not be a quadrotor.");
}

Eigen::VectorXd QuadrotorDynamicsSolver::f(const StateVector& x, const ControlVector& u)
{
    const Eigen::Vector3d translation = x.head<3>();
    const Eigen::Vector3d rpy = x.segment<3>(3);
    const Eigen::Vector3d v = x.segment<3>(6);
    const Eigen::Vector3d omega = x.tail<3>();

    // Create quaternion from rpy
    const Eigen::Quaterniond quaternion = Eigen::AngleAxisd(rpy(0), Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(rpy(1), Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(rpy(2), Eigen::Vector3d::UnitZ());

    const double F_1 = k_f_ * u(0);
    const double F_2 = k_f_ * u(1);
    const double F_3 = k_f_ * u(2);
    const double F_4 = k_f_ * u(3);
    const Eigen::Vector3d F(0, 0, F_1 + F_2 + F_3 + F_4);  // total rotor force in body frame

    const double M_1 = k_m_ * u(0);
    const double M_2 = k_m_ * u(1);
    const double M_3 = k_m_ * u(2);
    const double M_4 = k_m_ * u(3);
    const Eigen::Vector3d tau(L_ * (F_2 - F_4), L_ * (F_3 - F_1), (M_1 - M_2 + M_3 - M_4));  // total rotor torque in body frame

    const Eigen::Quaterniond dquaternion = (quaternion * Eigen::Quaterniond(0, omega(0), omega(1), omega(2)));
    const Eigen::Vector3d rpy_dot = dquaternion.toRotationMatrix().eulerAngles(0, 1, 2);

    StateVector x_dot(12);
    x_dot.head<3>() = v;                                                                 // velocity in world frame
    x_dot.segment<3>(3) = rpy_dot;                                                       // via quaternion derivative
    x_dot.segment<3>(6) = Eigen::Vector3d(0, 0, -g_) + (1. / mass_) * (quaternion * F);  // acceleration in world frame
    x_dot.segment<3>(9) = J_inv_ * (tau - omega.cross(J_ * omega));                      // Euler's equation : I* ω + ω x I* ω = constraint_decrease_ratio

    return x_dot;
}

Eigen::MatrixXd QuadrotorDynamicsSolver::fx(const StateVector& x, const ControlVector& u)
{
    // TODO
}

Eigen::MatrixXd QuadrotorDynamicsSolver::fu(const StateVector& x, const ControlVector& u)
{
    // TODO
}

}  // namespace exotica
<|endoftext|>


<commit_before>/* Copyright 2013 Telefonica Investigacion y Desarrollo, S.A.U
*
* This file is part of Orion Context Broker.
*
* Orion Context Broker is free software: you can redistribute it and/or
* modify it under the terms of the GNU Affero General Public License as
* published by the Free Software Foundation, either version 3 of the
* License, or (at your option) any later version.
*
* Orion Context Broker is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Affero
* General Public License for more details.
*
* You should have received a copy of the GNU Affero General Public License
* along with Orion Context Broker. If not, see http://www.gnu.org/licenses/.
*
* For those usages not covered by this license please contact with
* iot_support at tid dot es
*
* Author: Fermín Galán Márquez
*/
#include "logMsg/traceLevels.h"
#include "logMsg/logMsg.h"

#include "common/statistics.h"
#include "common/limits.h"
#include "common/globals.h"
#include "common/logTracing.h"
#include "alarmMgr/alarmMgr.h"
#include "rest/httpRequestSend.h"
#include "ngsiNotify/senderThread.h"
#include "cache/subCache.h"

/* ****************************************************************************
*
* startSenderThread -
*/
void* startSenderThread(void* p)
{
  std::vector<SenderThreadParams*>* paramsV = (std::vector<SenderThreadParams*>*) p;

  for (unsigned ix = 0; ix < paramsV->size(); ix++)
  {
    SenderThreadParams* params = (SenderThreadParams*) (*paramsV)[ix];
    char                portV[STRING_SIZE_FOR_INT];
    std::string         url;

    snprintf(portV, sizeof(portV), "%d", params->port);
    url = params->ip + ":" + portV + params->resource;

    strncpy(transactionId, params->transactionId, sizeof(transactionId));

    LM_T(LmtNotifier, ("sending to: host='%s', port=%d, verb=%s, tenant='%s', service-path: '%s', xauthToken: '%s', path='%s', content-type: %s",
                       params->ip.c_str(),
                       params->port,
                       params->verb.c_str(),
                       params->tenant.c_str(),
                       params->servicePath.c_str(),
                       params->xauthToken.c_str(),
                       params->resource.c_str(),
                       params->content_type.c_str()));

    long long    statusCode = -1;
    std::string  out;

    if (!simulatedNotification)
    {
      int          r;

      r = httpRequestSend(params->from,
                          params->ip,
                          params->port,
                          params->protocol,
                          params->verb,
                          params->tenant,
                          params->servicePath,
                          params->xauthToken,
                          params->resource,
                          params->content_type,
                          params->content,
                          params->fiwareCorrelator,
                          params->renderFormat,
                          &out,
                          &statusCode,
                          params->extraHeaders);

      if (r == 0)
      {
        statisticsUpdate(NotifyContextSent, params->mimeType);
        alarmMgr.forwardingErrorReset(url);

        if (params->registration == false)
        {
          subNotificationErrorStatus(params->tenant, params->subscriptionId, 0, statusCode, "");
        }
      }
      else
      {
        alarmMgr.forwardingError(url, "forwarding error: " + out);

        if (params->registration == false)
        {
          subNotificationErrorStatus(params->tenant, params->subscriptionId, -1, -1, out);
        }
      }
    }
    else
    {
      LM_T(LmtNotifier, ("simulatedNotification is 'true', skipping outgoing request"));
      __sync_fetch_and_add(&noOfSimulatedNotifications, 1);
    }

    // Add notificacion result summary in log INFO level
    if (statusCode != -1)
    {
      logInfoNotification(params->subscriptionId.c_str(), params->verb.c_str(), url.c_str(), statusCode);
    }
    else
    {
      logInfoNotification(params->subscriptionId.c_str(), params->verb.c_str(), url.c_str(), out.c_str());
    }

    // End transaction
    lmTransactionEnd();

    /* Delete the parameters after using them */
    delete params;
  }

  /* Delete the parameters vector after using it */
  delete paramsV;

  pthread_exit(NULL);
  return NULL;
}
<commit_msg>Update senderThread.cpp<commit_after>/* Copyright 2013 Telefonica Investigacion y Desarrollo, S.A.U
*
* This file is part of Orion Context Broker.
*
* Orion Context Broker is free software: you can redistribute it and/or
* modify it under the terms of the GNU Affero General Public License as
* published by the Free Software Foundation, either version 3 of the
* License, or (at your option) any later version.
*
* Orion Context Broker is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Affero
* General Public License for more details.
*
* You should have received a copy of the GNU Affero General Public License
* along with Orion Context Broker. If not, see http://www.gnu.org/licenses/.
*
* For those usages not covered by this license please contact with
* iot_support at tid dot es
*
* Author: Fermín Galán Márquez
*/
#include "logMsg/traceLevels.h"
#include "logMsg/logMsg.h"

#include "common/statistics.h"
#include "common/limits.h"
#include "common/globals.h"
#include "common/logTracing.h"
#include "alarmMgr/alarmMgr.h"
#include "rest/httpRequestSend.h"
#include "ngsiNotify/senderThread.h"
#include "cache/subCache.h"

/* ****************************************************************************
*
* startSenderThread -
*/
void* startSenderThread(void* p)
{
  std::vector<SenderThreadParams*>* paramsV = (std::vector<SenderThreadParams*>*) p;

  for (unsigned ix = 0; ix < paramsV->size(); ix++)
  {
    SenderThreadParams* params = (SenderThreadParams*) (*paramsV)[ix];
    char                portV[STRING_SIZE_FOR_INT];
    std::string         url;

    snprintf(portV, sizeof(portV), "%d", params->port);
    url = params->ip + ":" + portV + params->resource;

    strncpy(transactionId, params->transactionId, sizeof(transactionId));

    LM_T(LmtNotifier, ("sending to: host='%s', port=%d, verb=%s, tenant='%s', service-path: '%s', xauthToken: '%s', path='%s', content-type: %s",
                       params->ip.c_str(),
                       params->port,
                       params->verb.c_str(),
                       params->tenant.c_str(),
                       params->servicePath.c_str(),
                       params->xauthToken.c_str(),
                       params->resource.c_str(),
                       params->content_type.c_str()));

    long long    statusCode = -1;
    std::string  out;

    if (!simulatedNotification)
    {
      int          r;

      r = httpRequestSend(params->from,
                          params->ip,
                          params->port,
                          params->protocol,
                          params->verb,
                          params->tenant,
                          params->servicePath,
                          params->xauthToken,
                          params->resource,
                          params->content_type,
                          params->content,
                          params->fiwareCorrelator,
                          params->renderFormat,
                          &out,
                          &statusCode,
                          params->extraHeaders);

      if (r == 0)
      {
        statisticsUpdate(NotifyContextSent, params->mimeType);
        alarmMgr.notificationErrorReset(url);
        alarmMgr.forwardingErrorReset(url);

        if (params->registration == false)
        {
          subNotificationErrorStatus(params->tenant, params->subscriptionId, 0, statusCode, "");
        }
      }
      else
      {
        alarmMgr.notificationError(url, "notification failure for sender-thread: " + out);
        alarmMgr.forwardingError(url, "forwarding failure for sender-thread: " + out);

        if (params->registration == false)
        {
          subNotificationErrorStatus(params->tenant, params->subscriptionId, -1, -1, out);
        }
      }
    }
    else
    {
      LM_T(LmtNotifier, ("simulatedNotification is 'true', skipping outgoing request"));
      __sync_fetch_and_add(&noOfSimulatedNotifications, 1);
    }

    // Add notificacion result summary in log INFO level
    if (statusCode != -1)
    {
      logInfoNotification(params->subscriptionId.c_str(), params->verb.c_str(), url.c_str(), statusCode);
    }
    else
    {
      logInfoNotification(params->subscriptionId.c_str(), params->verb.c_str(), url.c_str(), out.c_str());
    }

    // End transaction
    lmTransactionEnd();

    /* Delete the parameters after using them */
    delete params;
  }

  /* Delete the parameters vector after using it */
  delete paramsV;

  pthread_exit(NULL);
  return NULL;
}
<|endoftext|>


<commit_before>#include "packettrain.h"

#include <QDebug>

PacketTrain::PacketTrain(QObject *parent)
: isInitialized(false)
, packetCounter(0)
, running(false)
{
    connect(&timer, SIGNAL(timeout()), this, SLOT(timeout()));
    connect(&timeouter, SIGNAL(timeout()), this, SLOT(stop()));

    timer.setInterval(10);

    timeouter.setInterval(2000);
    timeouter.setSingleShot(true);
}

PacketTrain::~PacketTrain()
{
}

QString PacketTrain::name() const
{
    return "packettrain";
}

bool PacketTrain::isMaster() const
{
    return master;
}

bool PacketTrain::initialize(const PeerList &peers, bool master, QUdpSocket *socket)
{
    if ( isInitialized )
        return false;

    if ( peers.isEmpty() ) {
        qDebug() << "Peer list is empty, can't initialize packettrain test";
        return false;
    }

    this->peers = peers;
    this->master = master;
    this->socket = socket;

    return (isInitialized=true);
}

void PacketTrain::uninitialize()
{
}

bool PacketTrain::start()
{
    qDebug() << Q_FUNC_INFO;

    running = true;

    timeouter.start();

    // Master waits for incoming packets
    if ( master ) {
        return true;
    }

    // Clients start sending data now
    timer.start();

    return true;
}

bool PacketTrain::stop()
{
    qDebug() << Q_FUNC_INFO;

    running = false;

    timer.stop();
    return true;
}

bool PacketTrain::isFinished() const
{
    return running;

    if ( master )
        return packetCounter >= 100;
    else
        return !timer.isActive();
}

void PacketTrain::processDatagram(const QByteArray &datagram, const QHostAddress &host, quint16 port)
{
    if ( master ) {
        emit packetCountChanged(packetCounter++);
    }

    timeouter.start();

    //qDebug() << Q_FUNC_INFO << host.toString() << port << packetCounter << "asdfff";
}

QVariant PacketTrain::data(int role) const
{
    Q_UNUSED(role)
    return QVariant();
}

QVariant PacketTrain::result() const
{
    QVariantMap values;
    values.insert("received-packets", packetCounter);
    return values;
}

void PacketTrain::timeout()
{
    emit packetCountChanged(packetCounter);
    if ( packetCounter++ >= 100 ) {
        stop();
        return;
    }

    QByteArray data;
    data.resize(512);
    data.fill('X');

    foreach(const Peer& peer, peers) {
        socket->writeDatagram(data, peer.host, peer.port);
    }
}
<commit_msg>PacketTrain forgot to derive from AbstractTest.<commit_after>#include "packettrain.h"

#include <QDebug>

PacketTrain::PacketTrain(QObject *parent)
: AbstractTest(parent)
, isInitialized(false)
, packetCounter(0)
, running(false)
{
    connect(&timer, SIGNAL(timeout()), this, SLOT(timeout()));
    connect(&timeouter, SIGNAL(timeout()), this, SLOT(stop()));

    timer.setInterval(10);

    timeouter.setInterval(2000);
    timeouter.setSingleShot(true);
}

PacketTrain::~PacketTrain()
{
}

QString PacketTrain::name() const
{
    return "packettrain";
}

bool PacketTrain::isMaster() const
{
    return master;
}

bool PacketTrain::initialize(const PeerList &peers, bool master, QUdpSocket *socket)
{
    if ( isInitialized )
        return false;

    if ( peers.isEmpty() ) {
        qDebug() << "Peer list is empty, can't initialize packettrain test";
        return false;
    }

    this->peers = peers;
    this->master = master;
    this->socket = socket;

    return (isInitialized=true);
}

void PacketTrain::uninitialize()
{
}

bool PacketTrain::start()
{
    qDebug() << Q_FUNC_INFO;

    running = true;

    timeouter.start();

    // Master waits for incoming packets
    if ( master ) {
        return true;
    }

    // Clients start sending data now
    timer.start();

    return true;
}

bool PacketTrain::stop()
{
    qDebug() << Q_FUNC_INFO;

    running = false;

    timer.stop();
    return true;
}

bool PacketTrain::isFinished() const
{
    return running;

    if ( master )
        return packetCounter >= 100;
    else
        return !timer.isActive();
}

void PacketTrain::processDatagram(const QByteArray &datagram, const QHostAddress &host, quint16 port)
{
    if ( master ) {
        emit packetCountChanged(packetCounter++);
    }

    timeouter.start();

    //qDebug() << Q_FUNC_INFO << host.toString() << port << packetCounter << "asdfff";
}

QVariant PacketTrain::data(int role) const
{
    Q_UNUSED(role)
    return QVariant();
}

QVariant PacketTrain::result() const
{
    QVariantMap values;
    values.insert("received-packets", packetCounter);
    return values;
}

void PacketTrain::timeout()
{
    emit packetCountChanged(packetCounter);
    if ( packetCounter++ >= 100 ) {
        stop();
        return;
    }

    QByteArray data;
    data.resize(512);
    data.fill('X');

    foreach(const Peer& peer, peers) {
        socket->writeDatagram(data, peer.host, peer.port);
    }
}
<|endoftext|>


<commit_before>#include "Control.h"

#include <sstream>

Control::Control() {

}

Control::Control(int id, HWND parent) :
_id(id),
_parent(parent) {
    _hWnd = GetDlgItem(parent, id);
}

Control::~Control() {

}

RECT Control::ScreenDimensions() {
    RECT r;
    GetWindowRect(_hWnd, &r);
    return r;
}

RECT Control::ClientDimensions() {
    RECT r;
    GetWindowRect(_hWnd, &r);
    int width = r.right - r.left;
    int height = r.bottom - r.top;

    POINT p = { r.left, r.top };
    int ret = ScreenToClient(_parent, &p);
    r.left = p.x;
    r.top = p.y;
    r.right = r.left + width;
    r.bottom = r.top + height;

    return r;
}

void Control::Enable() {
    EnableWindow(_hWnd, TRUE);
}

void Control::Disable() {
    EnableWindow(_hWnd, FALSE);
}

bool Control::Enabled() {
    return (IsWindowEnabled(_hWnd) == TRUE);
}

void Control::Enabled(bool enabled) {
    if (enabled == true) {
        Enable();
    } else {
        Disable();
    }
}

int Control::EmSize() {
    /* Determine the width of the text */
    HDC dc = GetDC(_hWnd);
    SIZE sz = { 0 };
    GetTextExtentPoint32(dc, L"M", 1, &sz);
    return sz.cx;
}

void Control::Move(int x, int y) {
    RECT r = ClientDimensions();
    MoveWindow(_hWnd, x, y, r.right - r.left, r.bottom - r.top, TRUE);
}

void Control::PlaceRightOf(Control &control) {
    RECT otherRect = control.ClientDimensions();
    CLOG(L"Other dims: %d %d %d %d", otherRect.top, otherRect.right, otherRect.bottom, otherRect.left);
    int x = otherRect.right;
    int y = ClientDimensions().top;
    Move(x, y);
}

bool Control::Text(std::wstring text) {
    return SetDlgItemText(_parent, _id, text.c_str()) == TRUE;
}

bool Control::Text(int value) {
    return SetDlgItemInt(_parent, _id, value, TRUE) == TRUE;
}

std::wstring Control::Text() {
    wchar_t text[MAX_EDITSTR];
    GetDlgItemText(_parent, _id, text, MAX_EDITSTR);
    return std::wstring(text);
}

int Control::TextAsInt() {
    std::wstring str = Text();
    int num;
    std::wistringstream wistr(str);
    wistr >> num;
    return num;
}

bool Control::Visible() {
    return IsWindowVisible(_hWnd) != FALSE;
}

void Control::Visible(bool visible) {
    ShowWindow(_hWnd, visible ? SW_SHOW : SW_HIDE);
}

void Control::AddWindowExStyle(long exStyle) {
    long exs = GetWindowLongPtr(_hWnd, GWL_EXSTYLE);
    exs |= exStyle;
    SetWindowLongPtr(_hWnd, GWL_EXSTYLE, exs);
}

void Control::RemoveWindowExStyle(long exStyle) {
    long exs = GetWindowLongPtr(_hWnd, GWL_EXSTYLE);
    exs &= ~exStyle;
    SetWindowLongPtr(_hWnd, GWL_EXSTYLE, exs);
}

DLGPROC Control::Command(unsigned short nCode) {
    /* By default, indicate that we did not process the message: */
    return FALSE;
}

DLGPROC Control::Notification(NMHDR *nHdr) {
    /* By default, indicate that we did not process the message: */
    return FALSE;
}
<commit_msg>Vertically center controls placed to the right and use a 1-em gap between the target control<commit_after>#include "Control.h"

#include <sstream>

Control::Control() {

}

Control::Control(int id, HWND parent) :
_id(id),
_parent(parent) {
    _hWnd = GetDlgItem(parent, id);
}

Control::~Control() {

}

RECT Control::ScreenDimensions() {
    RECT r;
    GetWindowRect(_hWnd, &r);
    return r;
}

RECT Control::ClientDimensions() {
    RECT r;
    GetWindowRect(_hWnd, &r);
    int width = r.right - r.left;
    int height = r.bottom - r.top;

    POINT p = { r.left, r.top };
    int ret = ScreenToClient(_parent, &p);
    r.left = p.x;
    r.top = p.y;
    r.right = r.left + width;
    r.bottom = r.top + height;

    return r;
}

void Control::Enable() {
    EnableWindow(_hWnd, TRUE);
}

void Control::Disable() {
    EnableWindow(_hWnd, FALSE);
}

bool Control::Enabled() {
    return (IsWindowEnabled(_hWnd) == TRUE);
}

void Control::Enabled(bool enabled) {
    if (enabled == true) {
        Enable();
    } else {
        Disable();
    }
}

int Control::EmSize() {
    /* Determine the width of the text */
    HDC dc = GetDC(_hWnd);
    SIZE sz = { 0 };
    GetTextExtentPoint32(dc, L"M", 1, &sz);
    return sz.cx;
}

void Control::Move(int x, int y) {
    RECT r = ClientDimensions();
    MoveWindow(_hWnd, x, y, r.right - r.left, r.bottom - r.top, TRUE);
}

void Control::PlaceRightOf(Control &control) {
    RECT otherRect = control.ClientDimensions();
    int otherHeight = otherRect.bottom - otherRect.top;
    /* Vertical center point of the other control: */
    int vCenter = otherRect.top + otherHeight / 2;

    RECT myRect = ClientDimensions();
    int myHeight = myRect.bottom - myRect.top;

    int x = otherRect.right + EmSize();
    int y = vCenter - myHeight / 2;
    Move(x, y);
}

bool Control::Text(std::wstring text) {
    return SetDlgItemText(_parent, _id, text.c_str()) == TRUE;
}

bool Control::Text(int value) {
    return SetDlgItemInt(_parent, _id, value, TRUE) == TRUE;
}

std::wstring Control::Text() {
    wchar_t text[MAX_EDITSTR];
    GetDlgItemText(_parent, _id, text, MAX_EDITSTR);
    return std::wstring(text);
}

int Control::TextAsInt() {
    std::wstring str = Text();
    int num;
    std::wistringstream wistr(str);
    wistr >> num;
    return num;
}

bool Control::Visible() {
    return IsWindowVisible(_hWnd) != FALSE;
}

void Control::Visible(bool visible) {
    ShowWindow(_hWnd, visible ? SW_SHOW : SW_HIDE);
}

void Control::AddWindowExStyle(long exStyle) {
    long exs = GetWindowLongPtr(_hWnd, GWL_EXSTYLE);
    exs |= exStyle;
    SetWindowLongPtr(_hWnd, GWL_EXSTYLE, exs);
}

void Control::RemoveWindowExStyle(long exStyle) {
    long exs = GetWindowLongPtr(_hWnd, GWL_EXSTYLE);
    exs &= ~exStyle;
    SetWindowLongPtr(_hWnd, GWL_EXSTYLE, exs);
}

DLGPROC Control::Command(unsigned short nCode) {
    /* By default, indicate that we did not process the message: */
    return FALSE;
}

DLGPROC Control::Notification(NMHDR *nHdr) {
    /* By default, indicate that we did not process the message: */
    return FALSE;
}
<|endoftext|>


<commit_before>/*
 * nt loader
 *
 * Copyright 2006-2008 Mike McCormack
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library 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; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA
 */

#include <stdio.h>
#include <stdarg.h>
#include <limits.h>
#include <fcntl.h>
#include <assert.h>
#include <stdlib.h>
#include <errno.h>
#include <sys/time.h>
#include <poll.h>
#include <signal.h>
#include <execinfo.h>

#include "ntstatus.h"
#define WIN32_NO_STATUS
#include "windef.h"
#include "winternl.h"

#include "debug.h"
#include "mem.h"
#include "object.h"
#include "objdir.h"
#include "ntcall.h"
#include "section.h"
#include "timer.h"
#include "unicode.h"
#include "fiber.h"
#include "file.h"
#include "event.h"
#include "symlink.h"

process_list_t processes;
thread_t *current;
object_t *ntdll_section;
int option_debug = 0;
ULONG KiIntSystemCall = 0;
bool forced_quit;

class default_sleeper_t : public sleeper_t
{
public:
	virtual bool sleep_timeout( LARGE_INTEGER& timeout );
	virtual ~default_sleeper_t() {}
};

int sleeper_t::get_int_timeout( LARGE_INTEGER& timeout )
{
	timeout.QuadPart = (timeout.QuadPart+9999)/10000;
	int t = INT_MAX;
	if (timeout.QuadPart < t)
		t = timeout.QuadPart;
	return t;
}

bool default_sleeper_t::sleep_timeout( LARGE_INTEGER& timeout )
{
	int t = get_int_timeout( timeout );
	int r = poll( 0, 0, t );
	if (r >= 0)
		return false;
	if (errno != EINTR)
		die("poll failed %d\n", errno);
	return false;
}

default_sleeper_t default_sleeper;
sleeper_t* sleeper = &default_sleeper;

int schedule(void)
{
	/* while there's still a thread running */
	while (processes.head())
	{
		// check if any timers have expired
		LARGE_INTEGER timeout;
		timeout_t::check_timers(timeout);

		// other fibers are active... schedule run them
		if (!fiber_t::last_fiber())
		{
			fiber_t::yield();
			continue;
		}

		// there's still processes but no active threads ... sleep
		if (timeout_t::check_timers(timeout))
		{
			if (sleeper->sleep_timeout(timeout))
				break;
		}
		else if (fiber_t::last_fiber())
			break;
	}

	return 0;
}

NTSTATUS create_initial_process( thread_t **t, UNICODE_STRING& us )
{
	BYTE *pstack;
	const unsigned int stack_size = 0x100 * PAGE_SIZE;
	process_t *p = NULL;
	CONTEXT ctx;
	INITIAL_TEB init_teb;
	CLIENT_ID id;
	object_t *section = NULL;
	file_t *file = 0;
	int r;

	r = open_file( file, us );
	if (r < STATUS_SUCCESS)
		return r;

	/* load the executable and ntdll */
	r = create_section( &section, file, 0, SEC_IMAGE, PAGE_EXECUTE_READWRITE );
	release( file );
	if (r < STATUS_SUCCESS)
		return r;

	/* create the initial process */
	r = create_process( &p, section );
	release( section );
	section = NULL;

	if (r < STATUS_SUCCESS)
		return r;

	PPEB ppeb = (PPEB) p->peb_section->get_kernel_address();
	p->create_exe_ppb( &ppeb->ProcessParameters, us );

	/* map the stack */
	pstack = NULL;
	r = p->vm->allocate_virtual_memory( &pstack, 0, stack_size, MEM_COMMIT | MEM_TOP_DOWN, PAGE_READWRITE );
	if (r < STATUS_SUCCESS)
		return r;

	/* teb initialization data */
	memset( &init_teb, 0, sizeof init_teb );
	init_teb.StackReserved = pstack;
	init_teb.StackCommit = (BYTE*)init_teb.StackReserved + stack_size;
	init_teb.StackCommitMax = (BYTE*)init_teb.StackCommit - PAGE_SIZE;

	/* initialize the first thread's context */
	p->vm->init_context( ctx );
	ctx.Eip = (DWORD) get_entry_point( p );
	ctx.Esp = (DWORD) pstack + stack_size - 8;

	dprintf("entry point = %08lx\n", ctx.Eip);

	/* when starting nt processes, make the PEB the first arg of NtProcessStartup */
	r = p->vm->copy_to_user( (BYTE*) ctx.Esp + 4, &p->PebBaseAddress, sizeof p->PebBaseAddress );

	if (r == STATUS_SUCCESS)
		r = create_thread( t, p, &id, &ctx, &init_teb, FALSE );

	release( p );

	return r;
}

NTSTATUS init_ntdll( void )
{
	WCHAR ntdll[] = {
		'\\','?','?','\\','c',':','\\','w','i','n','n','t','\\',
		's','y','s','t','e','m','3','2','\\','n','t','d','l','l','.','d','l','l',0 };
	unicode_string_t us;
	file_t *file = 0;
	NTSTATUS r;

	us.set( ntdll );

	r = open_file( file, us );
	if (r < STATUS_SUCCESS)
		die("failed to open ntdll\n");

	r = create_section( &ntdll_section, file, 0, SEC_IMAGE, PAGE_EXECUTE_READWRITE );
	if (r < STATUS_SUCCESS)
		die("failed to map ntdll\n");

	KiIntSystemCall = get_proc_address( ntdll_section, "KiIntSystemCall" );
	dprintf("KiIntSystemCall = %08lx\n", KiIntSystemCall);
	init_syscalls(KiIntSystemCall != 0);

	release( file );

	return r;
}

void free_ntdll( void )
{
	release( ntdll_section );
	ntdll_section = NULL;
}

void do_cleanup( void )
{
	int num_threads = 0, num_processes = 0;

	for ( process_iter_t pi(processes); pi; pi.next() )
	{
		process_t *p = pi;
		if (p->is_signalled())
			continue;
		num_processes++;
		fprintf(stderr, "process %04lx\n", p->id);
		for ( sibling_iter_t ti(p->threads); ti; ti.next() )
		{
			thread_t *t = ti;
			if (t->is_signalled())
				continue;
			fprintf(stderr, "\tthread %04lx\n", t->trace_id());
			num_threads++;
		}
	}
	if (num_threads || num_processes)
		fprintf(stderr, "%d threads %d processes left\n", num_threads, num_processes);
}

int usage( const char *prog )
{
	fprintf(stderr, "%s [-d] [-t] [-m] [-q] native.exe\n", prog );
	fprintf(stderr, "   [-d] break into debugger on exceptions\n");
	fprintf(stderr, "   [-m] memory reference counting\n");
	fprintf(stderr, "   [-t] trace syscall entry and exit\n");
	fprintf(stderr, "   [-q] quiet, suppress debug messages\n");
	return 0;
}

static void segv_handler(int)
{
	const int max_frames = 20;
	void *bt[max_frames];
	char **names;
	int n=0, size;
	ULONG id = 0;

	if (current)
		id = current->trace_id();

	size = backtrace(bt, max_frames);
	names = backtrace_symbols(bt, size);

	fprintf(stderr, "%04lx: caught kernel SEGV (%d frames):\n", id, size);
	for (n=0; n<size; n++)
	{
		fprintf(stderr, "%d: %s\n", n, names[n]);
	}
	exit(1);
}

bool init_skas();
bool init_tt();

bool option_skas = 1;

int main(int argc, char **argv)
{
	unicode_string_t us;
	thread_t *initial_thread = NULL;
	int r, n;

	// enable backtraces
	signal(SIGSEGV, segv_handler);

	for (n=1; n<argc; n++)
	{
		if (argv[n][0] != '-')
			break;
		switch (argv[n][1])
		{
		case 'd':
			option_debug = 1;
			break;
		case 't':
			option_trace = 1;
			break;
		case 'q':
			option_quiet = 1;
			break;
		case 's':
			option_skas = 0;
			break;
		default:
			return usage( argv[0] );
		}
	}

	if (n == argc)
		return usage( argv[0] );

	(option_skas && init_skas()) || init_tt();
	if (!pcreate_address_space)
		die("no way to manage address spaces found\n");

	// initialize boottime
	SYSTEM_TIME_OF_DAY_INFORMATION dummy;
	get_system_time_of_day( dummy );

	init_registry();
	fiber_t::fibers_init();
	init_root();
	create_directory_object( (PWSTR) L"\\" );
	create_directory_object( (PWSTR) L"\\??" );
	unicode_string_t link_name, link_target;
	link_name.set( L"\\DosDevices" );
	link_target.copy( L"\\??" );
	create_symlink( link_name, link_target );
	create_directory_object( (PWSTR) L"\\Device" );
	create_directory_object( (PWSTR) L"\\Device\\MailSlot" );
	create_directory_object( (PWSTR) L"\\Security" );
	//create_directory_object( (PWSTR) L"\\DosDevices" );
	create_directory_object( (PWSTR) L"\\BaseNamedObjects" );
	create_sync_event( (PWSTR) L"\\Security\\LSA_AUTHENTICATION_INITIALIZED" );
	create_sync_event( (PWSTR) L"\\SeLsaInitEvent" );
	init_random();
	init_pipe_device();
	// XP
	create_directory_object( (PWSTR) L"\\KernelObjects" );
	create_sync_event( (PWSTR) L"\\KernelObjects\\CritSecOutOfMemoryEvent" );
	init_drives();
	init_ntdll();
	create_kthread();

	us.copy( argv[n] );

	r = create_initial_process( &initial_thread, us );
	if (r < STATUS_SUCCESS)
		die("create_initial_process() failed (%08x)\n", r);

	// run the main loop
	schedule();

	ntgdi_fini();
	r = initial_thread->process->ExitStatus;
	//fprintf(stderr, "process exited (%08x)\n", r);
	release( initial_thread );

	shutdown_kthread();
	do_cleanup();

	free_root();
	fiber_t::fibers_finish();
	free_registry();
	free_ntdll();

	return r;
}
<commit_msg>Use getopt_long to parse options<commit_after>/*
 * nt loader
 *
 * Copyright 2006-2008 Mike McCormack
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library 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; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA
 */

#include "config.h"

#include <stdio.h>
#include <stdarg.h>
#include <limits.h>
#include <fcntl.h>
#include <assert.h>
#include <stdlib.h>
#include <errno.h>
#include <sys/time.h>
#include <poll.h>
#include <signal.h>
#include <execinfo.h>
#include <getopt.h>

#include "ntstatus.h"
#define WIN32_NO_STATUS
#include "windef.h"
#include "winternl.h"

#include "debug.h"
#include "mem.h"
#include "object.h"
#include "objdir.h"
#include "ntcall.h"
#include "section.h"
#include "timer.h"
#include "unicode.h"
#include "fiber.h"
#include "file.h"
#include "event.h"
#include "symlink.h"

process_list_t processes;
thread_t *current;
object_t *ntdll_section;
int option_debug = 0;
ULONG KiIntSystemCall = 0;
bool forced_quit;

class default_sleeper_t : public sleeper_t
{
public:
	virtual bool sleep_timeout( LARGE_INTEGER& timeout );
	virtual ~default_sleeper_t() {}
};

int sleeper_t::get_int_timeout( LARGE_INTEGER& timeout )
{
	timeout.QuadPart = (timeout.QuadPart+9999)/10000;
	int t = INT_MAX;
	if (timeout.QuadPart < t)
		t = timeout.QuadPart;
	return t;
}

bool default_sleeper_t::sleep_timeout( LARGE_INTEGER& timeout )
{
	int t = get_int_timeout( timeout );
	int r = poll( 0, 0, t );
	if (r >= 0)
		return false;
	if (errno != EINTR)
		die("poll failed %d\n", errno);
	return false;
}

default_sleeper_t default_sleeper;
sleeper_t* sleeper = &default_sleeper;

int schedule(void)
{
	/* while there's still a thread running */
	while (processes.head())
	{
		// check if any timers have expired
		LARGE_INTEGER timeout;
		timeout_t::check_timers(timeout);

		// other fibers are active... schedule run them
		if (!fiber_t::last_fiber())
		{
			fiber_t::yield();
			continue;
		}

		// there's still processes but no active threads ... sleep
		if (timeout_t::check_timers(timeout))
		{
			if (sleeper->sleep_timeout(timeout))
				break;
		}
		else if (fiber_t::last_fiber())
			break;
	}

	return 0;
}

NTSTATUS create_initial_process( thread_t **t, UNICODE_STRING& us )
{
	BYTE *pstack;
	const unsigned int stack_size = 0x100 * PAGE_SIZE;
	process_t *p = NULL;
	CONTEXT ctx;
	INITIAL_TEB init_teb;
	CLIENT_ID id;
	object_t *section = NULL;
	file_t *file = 0;
	int r;

	r = open_file( file, us );
	if (r < STATUS_SUCCESS)
		return r;

	/* load the executable and ntdll */
	r = create_section( &section, file, 0, SEC_IMAGE, PAGE_EXECUTE_READWRITE );
	release( file );
	if (r < STATUS_SUCCESS)
		return r;

	/* create the initial process */
	r = create_process( &p, section );
	release( section );
	section = NULL;

	if (r < STATUS_SUCCESS)
		return r;

	PPEB ppeb = (PPEB) p->peb_section->get_kernel_address();
	p->create_exe_ppb( &ppeb->ProcessParameters, us );

	/* map the stack */
	pstack = NULL;
	r = p->vm->allocate_virtual_memory( &pstack, 0, stack_size, MEM_COMMIT | MEM_TOP_DOWN, PAGE_READWRITE );
	if (r < STATUS_SUCCESS)
		return r;

	/* teb initialization data */
	memset( &init_teb, 0, sizeof init_teb );
	init_teb.StackReserved = pstack;
	init_teb.StackCommit = (BYTE*)init_teb.StackReserved + stack_size;
	init_teb.StackCommitMax = (BYTE*)init_teb.StackCommit - PAGE_SIZE;

	/* initialize the first thread's context */
	p->vm->init_context( ctx );
	ctx.Eip = (DWORD) get_entry_point( p );
	ctx.Esp = (DWORD) pstack + stack_size - 8;

	dprintf("entry point = %08lx\n", ctx.Eip);

	/* when starting nt processes, make the PEB the first arg of NtProcessStartup */
	r = p->vm->copy_to_user( (BYTE*) ctx.Esp + 4, &p->PebBaseAddress, sizeof p->PebBaseAddress );

	if (r == STATUS_SUCCESS)
		r = create_thread( t, p, &id, &ctx, &init_teb, FALSE );

	release( p );

	return r;
}

NTSTATUS init_ntdll( void )
{
	WCHAR ntdll[] = {
		'\\','?','?','\\','c',':','\\','w','i','n','n','t','\\',
		's','y','s','t','e','m','3','2','\\','n','t','d','l','l','.','d','l','l',0 };
	unicode_string_t us;
	file_t *file = 0;
	NTSTATUS r;

	us.set( ntdll );

	r = open_file( file, us );
	if (r < STATUS_SUCCESS)
		die("failed to open ntdll\n");

	r = create_section( &ntdll_section, file, 0, SEC_IMAGE, PAGE_EXECUTE_READWRITE );
	if (r < STATUS_SUCCESS)
		die("failed to map ntdll\n");

	KiIntSystemCall = get_proc_address( ntdll_section, "KiIntSystemCall" );
	dprintf("KiIntSystemCall = %08lx\n", KiIntSystemCall);
	init_syscalls(KiIntSystemCall != 0);

	release( file );

	return r;
}

void free_ntdll( void )
{
	release( ntdll_section );
	ntdll_section = NULL;
}

void do_cleanup( void )
{
	int num_threads = 0, num_processes = 0;

	for ( process_iter_t pi(processes); pi; pi.next() )
	{
		process_t *p = pi;
		if (p->is_signalled())
			continue;
		num_processes++;
		fprintf(stderr, "process %04lx\n", p->id);
		for ( sibling_iter_t ti(p->threads); ti; ti.next() )
		{
			thread_t *t = ti;
			if (t->is_signalled())
				continue;
			fprintf(stderr, "\tthread %04lx\n", t->trace_id());
			num_threads++;
		}
	}
	if (num_threads || num_processes)
		fprintf(stderr, "%d threads %d processes left\n", num_threads, num_processes);
}

static void segv_handler(int)
{
	const int max_frames = 20;
	void *bt[max_frames];
	char **names;
	int n=0, size;
	ULONG id = 0;

	if (current)
		id = current->trace_id();

	size = backtrace(bt, max_frames);
	names = backtrace_symbols(bt, size);

	fprintf(stderr, "%04lx: caught kernel SEGV (%d frames):\n", id, size);
	for (n=0; n<size; n++)
	{
		fprintf(stderr, "%d: %s\n", n, names[n]);
	}
	exit(1);
}

bool init_skas();
bool init_tt();

void usage( void )
{
	const char usage[] =
		"Usage: %s [options] [native.exe]\n"
		"Options:\n"
		"  -d,--debug    break into debugger on exceptions\n"
		"  -h,--help     print this message\n"
		"  -q,--quiet    quiet, suppress debug messages\n"
		"  -t,--trace    trace syscall entry and exit\n"
		"  -v,--version  print version\n\n"
		"  smss.exe is started by default\n\n";
	printf( usage, PACKAGE_NAME );
	exit(0);
}

void version( void )
{
	const char version[] = "%s\n"
		"Copyright (C) 2008 Mike McCormack\n"
		"Licence LGPL\n"
		"This is free software: you are free to change and redistribute it.\n"
		"There is NO WARRANTY, to the extent permitted by law.\n\n";
	printf( version, PACKAGE_STRING );
	exit(0);
}

void parse_options(int argc, char **argv)
{
	while (1)
	{
		int option_index;
		static struct option long_options[] = {
			{"debug", 0, &option_debug, 1 },
			{"help", 0, 0, 'h' },
			{"quiet", 0, &option_quiet, 1 },
			{"trace", 0, &option_trace, 1 },
			{"version", 0, 0, 'v' },
			{NULL, 0, 0, 0 },
		};

		int ch = getopt_long(argc, argv, "dhtqv", long_options, &option_index );
		if (ch == -1)
			break;

		switch (ch)
		{
		case 'd':
			option_debug = 1;
			break;
		case 'h':
			usage();
			break;
		case 'q':
			option_quiet = 1;
			break;
		case 't':
			option_trace = 1;
			break;
		case 'v':
			version();
		}
	}
}

int main(int argc, char **argv)
{
	unicode_string_t us;
	thread_t *initial_thread = NULL;
	const char *exename;

	parse_options( argc, argv );

	if (optind == argc)
	{
		// default to starting smss.exe
		exename = "\\??\\c:\\winnt\\system32\\smss.exe";
	}
	else
	{
		exename = argv[optind];
	}

	// the skas3 patch is deprecated...
	if (0) init_skas();

	init_tt();
	if (!pcreate_address_space)
		die("no way to manage address spaces found\n");

	// enable backtraces
	signal(SIGSEGV, segv_handler);

	// initialize boottime
	SYSTEM_TIME_OF_DAY_INFORMATION dummy;
	get_system_time_of_day( dummy );

	init_registry();
	fiber_t::fibers_init();
	init_root();
	create_directory_object( (PWSTR) L"\\" );
	create_directory_object( (PWSTR) L"\\??" );
	unicode_string_t link_name, link_target;
	link_name.set( L"\\DosDevices" );
	link_target.copy( L"\\??" );
	create_symlink( link_name, link_target );
	create_directory_object( (PWSTR) L"\\Device" );
	create_directory_object( (PWSTR) L"\\Device\\MailSlot" );
	create_directory_object( (PWSTR) L"\\Security" );
	//create_directory_object( (PWSTR) L"\\DosDevices" );
	create_directory_object( (PWSTR) L"\\BaseNamedObjects" );
	create_sync_event( (PWSTR) L"\\Security\\LSA_AUTHENTICATION_INITIALIZED" );
	create_sync_event( (PWSTR) L"\\SeLsaInitEvent" );
	init_random();
	init_pipe_device();
	// XP
	create_directory_object( (PWSTR) L"\\KernelObjects" );
	create_sync_event( (PWSTR) L"\\KernelObjects\\CritSecOutOfMemoryEvent" );
	init_drives();
	init_ntdll();
	create_kthread();

	us.copy( exename );

	int r = create_initial_process( &initial_thread, us );
	if (r < STATUS_SUCCESS)
		die("create_initial_process() failed (%08x)\n", r);

	// run the main loop
	schedule();

	ntgdi_fini();
	r = initial_thread->process->ExitStatus;
	//fprintf(stderr, "process exited (%08x)\n", r);
	release( initial_thread );

	shutdown_kthread();
	do_cleanup();

	free_root();
	fiber_t::fibers_finish();
	free_registry();
	free_ntdll();

	return r;
}
<|endoftext|>


<commit_before>
#include "pch.h"
#include "D2DWindow.h"
#include "Win32Defs.h"

using namespace std;
using namespace D2D1;

#pragma comment (lib, "d2d1.lib")
#pragma comment (lib, "dwrite.lib")
#pragma comment (lib, "D3D11.lib")
#pragma comment (lib, "Dxgi.lib")

static ATOM WndClassAtom;
static const wchar_t WndClassName[] = L"D2DWindow-{175802BE-0628-45C0-BC8A-3D27C6F4F0BE}";

D2DWindow::D2DWindow (HINSTANCE hInstance, DWORD exStyle, DWORD style, const RECT& rect, HWND hWndParent, HMENU hMenuOrControlId, IDWriteFactory* dWriteFactory)
	: base(hInstance, WndClassName, exStyle, style, rect, hWndParent, hMenuOrControlId)
	, _dWriteFactory(dWriteFactory)
{
	auto hr = D2D1CreateFactory(D2D1_FACTORY_TYPE_MULTI_THREADED, IID_PPV_ARGS(&_d2dFactory));
	ThrowIfFailed(hr);

	CreateD2DDeviceContext();

	float dpiX, dpiY;
	_renderTarget->GetDpi(&dpiX, &dpiY);
	_clientSizeDips.width = GetClientWidthPixels() * 96.0f / dpiX;
	_clientSizeDips.height = GetClientHeightPixels() * 96.0f / dpiY;
}

void D2DWindow::CreateD2DDeviceContext()
{
	D2D1_RENDER_TARGET_PROPERTIES props = {};
	props.type = D2D1_RENDER_TARGET_TYPE_HARDWARE;
	props.pixelFormat.format = DXGI_FORMAT_B8G8R8A8_UNORM;
	props.pixelFormat.alphaMode = D2D1_ALPHA_MODE_IGNORE;
	_d2dFactory->GetDesktopDpi (&props.dpiX, &props.dpiY);
	props.usage = D2D1_RENDER_TARGET_USAGE_NONE;
	props.minLevel = D2D1_FEATURE_LEVEL_DEFAULT;

	D2D1_HWND_RENDER_TARGET_PROPERTIES hwndrtprops = { };
	hwndrtprops.hwnd = GetHWnd();
	hwndrtprops.pixelSize = { (UINT32) GetClientWidthPixels(), (UINT32) GetClientHeightPixels() };

	auto hr = _d2dFactory->CreateHwndRenderTarget(&props, &hwndrtprops, &_renderTarget);
	ThrowIfFailed(hr);
}

std::optional<LRESULT> D2DWindow::WindowProc(HWND hwnd, UINT uMsg, WPARAM wParam, LPARAM lParam)
{
	auto resultBaseClass = base::WindowProc(hwnd, uMsg, wParam, lParam);

	if (uMsg == WM_SIZE)
	{
		_renderTarget = nullptr;
		CreateD2DDeviceContext();

		float dpiX, dpiY;
		_renderTarget->GetDpi(&dpiX, &dpiY);
		_clientSizeDips.width = GetClientWidthPixels() * 96.0f / dpiX;
		_clientSizeDips.height = GetClientHeightPixels() * 96.0f / dpiY;

		return 0;
	}

	if (uMsg == WM_ERASEBKGND)
		return 0; // 0 means the window remains marked for erasing, so the fErase member of the PAINTSTRUCT structure will be TRUE.

	if (uMsg == WM_PAINT)
	{
		ProcessWmPaint(hwnd);
		return 0;
	}

	return std::nullopt;
}

void D2DWindow::ProcessWmPaint (HWND hwnd)
{
	if (_painting)
	{
		// We get here when we're called recursively. The only such case I've seen so far is when
		// an assertion fails in code called from this function. We don't want to restart painting
		// cause we'll end up with a stack overflow, so let's return without attempting anything "smart".
		return;
	}

	HRESULT hr;

	// Call this before calculating the update rects, to allow derived classed to invalidate stuff.
	this->OnBeforeRender();

	// -------------------------------------------------
	// draw the stuff

	// Problem: If an assertion fails in code called from this function, the C++ runtime will try to display
	// the assertion message box. It seems that Windows, while processing WM_PAINT, displays message boxes
	// only if the application has called BeginPaint; if the application has not called BeginPaint, Windows
	// will not display the message box, will make sounds when clicking on the application window, and will
	// wait for the user to press Alt before finally displaying it (go figure!)

	PAINTSTRUCT ps;
	::BeginPaint(hwnd, &ps); // this will also hide the caret, if shown.

	_painting = true;

	_renderTarget->BeginDraw();
	_renderTarget->SetTransform(IdentityMatrix());

	this->Render(_renderTarget);

	hr = _renderTarget->EndDraw(); ThrowIfFailed(hr);

	::EndPaint(hwnd, &ps); // this will show the caret in case BeginPaint above hid it.

	this->OnAfterRender();

	assert(_painting);
	_painting = false;
}

D2D1_POINT_2F D2DWindow::GetDipLocationFromPixelLocation(POINT p) const
{
	float dpiX, dpiY;
	_renderTarget->GetDpi(&dpiX, &dpiY);
	return D2D1_POINT_2F{ p.x * 96.0f / dpiX, p.y * 96.0f / dpiY };
}

D2D1_POINT_2F D2DWindow::GetDipLocationFromPixelLocation(float xPixels, float yPixels) const
{
	float dpiX, dpiY;
	_renderTarget->GetDpi(&dpiX, &dpiY);
	return D2D1_POINT_2F{ xPixels * 96.0f / dpiX, yPixels * 96.0f / dpiY };
}

POINT D2DWindow::GetPixelLocationFromDipLocation(D2D1_POINT_2F locationDips) const
{
	float dpiX, dpiY;
	_renderTarget->GetDpi(&dpiX, &dpiY);
	return POINT{ (int)roundf(locationDips.x / 96.0f * dpiX), (int)roundf(locationDips.y / 96.0f * dpiY) };
}

D2D1_SIZE_F D2DWindow::GetDipSizeFromPixelSize(SIZE sz) const
{
	float dpiX, dpiY;
	_renderTarget->GetDpi(&dpiX, &dpiY);
	return D2D1_SIZE_F{ sz.cx * 96.0f / dpiX, sz.cy * 96.0f / dpiY };
}

SIZE D2DWindow::GetPixelSizeFromDipSize(D2D1_SIZE_F sizeDips) const
{
	float dpiX, dpiY;
	_renderTarget->GetDpi(&dpiX, &dpiY);
	return SIZE{ (int)(sizeDips.width / 96.0f * dpiX), (int)(sizeDips.height / 96.0f * dpiY) };
}

ColorF GetD2DSystemColor (int sysColorIndex)
{
	DWORD brg = GetSysColor (sysColorIndex);
	DWORD rgb = ((brg & 0xff0000) >> 16) | (brg & 0xff00) | ((brg & 0xff) << 16);
	return ColorF (rgb);
}

TextLayout TextLayout::Create (IDWriteFactory* dWriteFactory, IDWriteTextFormat* format, const wchar_t* str, float maxWidth)
{
	IDWriteTextLayoutPtr tl;
	auto hr = dWriteFactory->CreateTextLayout(str, (UINT32) wcslen(str), format, (maxWidth != 0) ? maxWidth : 10000, 10000, &tl); ThrowIfFailed(hr);
	DWRITE_TEXT_METRICS metrics;
	hr = tl->GetMetrics(&metrics); ThrowIfFailed(hr);
	return TextLayout { move(tl), metrics };
}

<commit_msg>Cleanup of unused libs.<commit_after>
#include "pch.h"
#include "D2DWindow.h"
#include "Win32Defs.h"

using namespace std;
using namespace D2D1;

#pragma comment (lib, "d2d1.lib")
#pragma comment (lib, "dwrite.lib")

static ATOM WndClassAtom;
static const wchar_t WndClassName[] = L"D2DWindow-{175802BE-0628-45C0-BC8A-3D27C6F4F0BE}";

D2DWindow::D2DWindow (HINSTANCE hInstance, DWORD exStyle, DWORD style, const RECT& rect, HWND hWndParent, HMENU hMenuOrControlId, IDWriteFactory* dWriteFactory)
	: base(hInstance, WndClassName, exStyle, style, rect, hWndParent, hMenuOrControlId)
	, _dWriteFactory(dWriteFactory)
{
	auto hr = D2D1CreateFactory(D2D1_FACTORY_TYPE_MULTI_THREADED, IID_PPV_ARGS(&_d2dFactory));
	ThrowIfFailed(hr);

	CreateD2DDeviceContext();

	float dpiX, dpiY;
	_renderTarget->GetDpi(&dpiX, &dpiY);
	_clientSizeDips.width = GetClientWidthPixels() * 96.0f / dpiX;
	_clientSizeDips.height = GetClientHeightPixels() * 96.0f / dpiY;
}

void D2DWindow::CreateD2DDeviceContext()
{
	D2D1_RENDER_TARGET_PROPERTIES props = {};
	props.type = D2D1_RENDER_TARGET_TYPE_HARDWARE;
	props.pixelFormat.format = DXGI_FORMAT_B8G8R8A8_UNORM;
	props.pixelFormat.alphaMode = D2D1_ALPHA_MODE_IGNORE;
	_d2dFactory->GetDesktopDpi (&props.dpiX, &props.dpiY);
	props.usage = D2D1_RENDER_TARGET_USAGE_NONE;
	props.minLevel = D2D1_FEATURE_LEVEL_DEFAULT;

	D2D1_HWND_RENDER_TARGET_PROPERTIES hwndrtprops = { };
	hwndrtprops.hwnd = GetHWnd();
	hwndrtprops.pixelSize = { (UINT32) GetClientWidthPixels(), (UINT32) GetClientHeightPixels() };

	auto hr = _d2dFactory->CreateHwndRenderTarget(&props, &hwndrtprops, &_renderTarget);
	ThrowIfFailed(hr);
}

std::optional<LRESULT> D2DWindow::WindowProc(HWND hwnd, UINT uMsg, WPARAM wParam, LPARAM lParam)
{
	auto resultBaseClass = base::WindowProc(hwnd, uMsg, wParam, lParam);

	if (uMsg == WM_SIZE)
	{
		_renderTarget = nullptr;
		CreateD2DDeviceContext();

		float dpiX, dpiY;
		_renderTarget->GetDpi(&dpiX, &dpiY);
		_clientSizeDips.width = GetClientWidthPixels() * 96.0f / dpiX;
		_clientSizeDips.height = GetClientHeightPixels() * 96.0f / dpiY;

		return 0;
	}

	if (uMsg == WM_ERASEBKGND)
		return 0; // 0 means the window remains marked for erasing, so the fErase member of the PAINTSTRUCT structure will be TRUE.

	if (uMsg == WM_PAINT)
	{
		ProcessWmPaint(hwnd);
		return 0;
	}

	return std::nullopt;
}

void D2DWindow::ProcessWmPaint (HWND hwnd)
{
	if (_painting)
	{
		// We get here when we're called recursively. The only such case I've seen so far is when
		// an assertion fails in code called from this function. We don't want to restart painting
		// cause we'll end up with a stack overflow, so let's return without attempting anything "smart".
		return;
	}

	HRESULT hr;

	// Call this before calculating the update rects, to allow derived classed to invalidate stuff.
	this->OnBeforeRender();

	// -------------------------------------------------
	// draw the stuff

	// Problem: If an assertion fails in code called from this function, the C++ runtime will try to display
	// the assertion message box. It seems that Windows, while processing WM_PAINT, displays message boxes
	// only if the application has called BeginPaint; if the application has not called BeginPaint, Windows
	// will not display the message box, will make sounds when clicking on the application window, and will
	// wait for the user to press Alt before finally displaying it (go figure!)

	PAINTSTRUCT ps;
	::BeginPaint(hwnd, &ps); // this will also hide the caret, if shown.

	_painting = true;

	_renderTarget->BeginDraw();
	_renderTarget->SetTransform(IdentityMatrix());

	this->Render(_renderTarget);

	hr = _renderTarget->EndDraw(); ThrowIfFailed(hr);

	::EndPaint(hwnd, &ps); // this will show the caret in case BeginPaint above hid it.

	this->OnAfterRender();

	assert(_painting);
	_painting = false;
}

D2D1_POINT_2F D2DWindow::GetDipLocationFromPixelLocation(POINT p) const
{
	float dpiX, dpiY;
	_renderTarget->GetDpi(&dpiX, &dpiY);
	return D2D1_POINT_2F{ p.x * 96.0f / dpiX, p.y * 96.0f / dpiY };
}

D2D1_POINT_2F D2DWindow::GetDipLocationFromPixelLocation(float xPixels, float yPixels) const
{
	float dpiX, dpiY;
	_renderTarget->GetDpi(&dpiX, &dpiY);
	return D2D1_POINT_2F{ xPixels * 96.0f / dpiX, yPixels * 96.0f / dpiY };
}

POINT D2DWindow::GetPixelLocationFromDipLocation(D2D1_POINT_2F locationDips) const
{
	float dpiX, dpiY;
	_renderTarget->GetDpi(&dpiX, &dpiY);
	return POINT{ (int)roundf(locationDips.x / 96.0f * dpiX), (int)roundf(locationDips.y / 96.0f * dpiY) };
}

D2D1_SIZE_F D2DWindow::GetDipSizeFromPixelSize(SIZE sz) const
{
	float dpiX, dpiY;
	_renderTarget->GetDpi(&dpiX, &dpiY);
	return D2D1_SIZE_F{ sz.cx * 96.0f / dpiX, sz.cy * 96.0f / dpiY };
}

SIZE D2DWindow::GetPixelSizeFromDipSize(D2D1_SIZE_F sizeDips) const
{
	float dpiX, dpiY;
	_renderTarget->GetDpi(&dpiX, &dpiY);
	return SIZE{ (int)(sizeDips.width / 96.0f * dpiX), (int)(sizeDips.height / 96.0f * dpiY) };
}

ColorF GetD2DSystemColor (int sysColorIndex)
{
	DWORD brg = GetSysColor (sysColorIndex);
	DWORD rgb = ((brg & 0xff0000) >> 16) | (brg & 0xff00) | ((brg & 0xff) << 16);
	return ColorF (rgb);
}

TextLayout TextLayout::Create (IDWriteFactory* dWriteFactory, IDWriteTextFormat* format, const wchar_t* str, float maxWidth)
{
	IDWriteTextLayoutPtr tl;
	auto hr = dWriteFactory->CreateTextLayout(str, (UINT32) wcslen(str), format, (maxWidth != 0) ? maxWidth : 10000, 10000, &tl); ThrowIfFailed(hr);
	DWRITE_TEXT_METRICS metrics;
	hr = tl->GetMetrics(&metrics); ThrowIfFailed(hr);
	return TextLayout { move(tl), metrics };
}

<|endoftext|>


<commit_before>#include <assert.h>
#include <iostream>
#include <memory>
#include <vector>

#include "common/simhubdeviceplugin.h"
#include "main.h"

// -- public C FFI

extern "C" {
int simplug_init(SPHANDLE *plugin_instance, LoggingFunctionCB logger)
{
    *plugin_instance = new SimSourcePluginStateManager(logger);
    return 0;
}

int simplug_config_passthrough(SPHANDLE plugin_instance, void *libconfig_instance)
{
    return static_cast<PluginStateManager *>(plugin_instance)->configPassthrough(static_cast<libconfig::Config *>(libconfig_instance));
}

int simplug_preflight_complete(SPHANDLE plugin_instance)
{
    return static_cast<PluginStateManager *>(plugin_instance)->preflightComplete();
}

void simplug_commence_eventing(SPHANDLE plugin_instance, EnqueueEventHandler enqueue_callback, void *arg)
{
    static_cast<PluginStateManager *>(plugin_instance)->commenceEventing(enqueue_callback, arg);
}

int simplug_deliver_value(SPHANDLE plugin_instance, GenericTLV *value)
{
    return static_cast<PluginStateManager *>(plugin_instance)->deliverValue(value);
}

void simplug_cease_eventing(SPHANDLE plugin_instance)
{
    static_cast<PluginStateManager *>(plugin_instance)->ceaseEventing();
}

void simplug_release(SPHANDLE plugin_instance)
{
    assert(plugin_instance);
    delete static_cast<PluginStateManager *>(plugin_instance);
}
}

// -- internal implementation

SimSourcePluginStateManager *SimSourcePluginStateManager::_StateManagerInstance = NULL;

void SimSourcePluginStateManager::AllocBuffer(uv_handle_t *handle, size_t size, uv_buf_t *buf)
{
    *buf = SimSourcePluginStateManager::StateManagerInstance()->_readBuffer;
}

//! static getter for singleton instance of our class
SimSourcePluginStateManager *SimSourcePluginStateManager::StateManagerInstance(void)
{
    return _StateManagerInstance;
}

SimSourcePluginStateManager::SimSourcePluginStateManager(LoggingFunctionCB logger)
    : PluginStateManager(logger)
{
    // enforce singleton pre-condition

    assert(!_StateManagerInstance);

    _StateManagerInstance = this;
    _processedElementCount = 0;

    if (!(_rawBuffer = (char *)malloc(BUFFER_LEN))) {
        printf("Unable to allocate buffer of size %d", BUFFER_LEN);
    }
    else {
        _readBuffer = ::uv_buf_init(_rawBuffer, BUFFER_LEN);
    }
}

SimSourcePluginStateManager::~SimSourcePluginStateManager(void)
{
    // TODO: enable once shtudown implemented
    if (_pluginThread != NULL) {
        if (_pluginThread->joinable()) {
            ceaseEventing();
            _pluginThread->join();
        }

        delete _pluginThread;
    }

    if (_rawBuffer != NULL)
        free(_rawBuffer);
}

int SimSourcePluginStateManager::preflightComplete(void)
{
    int retVal = PREFLIGHT_OK;
    int port = 8091;

    libconfig::Setting *devicesConfiguraiton = NULL;

    std::string type = "";
    std::string ipAddress = "";

    try {
        devicesConfiguraiton = &_config->lookup("configuration");
    }
    catch (const libconfig::SettingNotFoundException &nfex) {
        _logger(LOG_ERROR, "Config file parse error at %s. Skipping....", nfex.getPath());
    }

    for (libconfig::SettingIterator iter = devicesConfiguraiton->begin(); iter != devicesConfiguraiton->end(); iter++) {

        if (iter->exists("ipAddress")) {
            iter->lookupValue("ipAddress", ipAddress);
        }
        else {
            ipAddress = "127.0.0.1";
        }

        if (iter->exists("port")) {
            iter->lookupValue("port", port);
        }
    }

    struct sockaddr_in req_addr;

    _eventLoop = uv_default_loop();
    check_uv(uv_loop_init(_eventLoop));

    check_uv(uv_tcp_init(_eventLoop, &_tcpClient));
    uv_tcp_keepalive(&_tcpClient, 1, 60);

    uv_ip4_addr(ipAddress.c_str(), port, &req_addr);

    int connectErr = uv_tcp_connect(&_connectReq, &_tcpClient, (struct sockaddr *)&req_addr, &SimSourcePluginStateManager::OnConnect);

    if (connectErr < 0) {
        retVal = PREFLIGHT_FAIL;
    }

    return retVal;
}

void SimSourcePluginStateManager::OnConnect(uv_connect_t *req, int status)
{
    assert(SimSourcePluginStateManager::StateManagerInstance());

    if (status == SIM_CONNECT_NOT_FOUND) {
        SimSourcePluginStateManager::StateManagerInstance()->_logger(LOG_ERROR, "   - Failed to connect to simulator");
    }
    else {
        SimSourcePluginStateManager::StateManagerInstance()->_logger(LOG_INFO, " - Connected to simulator %d", status);
        SimSourcePluginStateManager::StateManagerInstance()->instanceConnectionHandler(req, status);
    }
}

void SimSourcePluginStateManager::OnRead(uv_stream_t *server, ssize_t nread, const uv_buf_t *buf)
{
    SimSourcePluginStateManager::StateManagerInstance()->instanceReadHandler(server, nread, buf);
}

void SimSourcePluginStateManager::OnClose(uv_handle_t *handle)
{
    assert(SimSourcePluginStateManager::StateManagerInstance());
    SimSourcePluginStateManager::StateManagerInstance()->instanceCloseHandler(handle);
}

void SimSourcePluginStateManager::instanceConnectionHandler(uv_connect_t *req, int status)
{
    if (uv_is_readable(req->handle)) {
        uv_read_start(req->handle, &SimSourcePluginStateManager::AllocBuffer, &SimSourcePluginStateManager::OnRead);
    }
    else {
        printf("not readable\n");
    }
}

void SimSourcePluginStateManager::instanceReadHandler(uv_stream_t *server, ssize_t nread, const uv_buf_t *buf)
{
    if (nread > 0) {
        uv_buf_t buffer = uv_buf_init((char *)malloc(nread), nread);
        memcpy(buffer.base, buf->base, nread);
        buffer.base[nread - 1] = '\0';
        processData(buffer.base, nread);
        free(buffer.base);
    }
    else if (nread < 0) {
        if (nread == UV_EOF) {
            SimSourcePluginStateManager::StateManagerInstance()->_logger(LOG_INFO, " - Stopping prepare3d ingest loop");
            ceaseEventing();
        }
        else {
            SimSourcePluginStateManager::StateManagerInstance()->_logger(LOG_INFO, " - %s", uv_strerror(nread));
            uv_close((uv_handle_t *)server, &SimSourcePluginStateManager::OnClose);
        }
    }
}

void SimSourcePluginStateManager::processData(char *data, int len)
{
    if (len > 2) {
        int elementCount = 0;
        char *p = strtok(data, "\n");
        char *array[MAX_ELEMENTS_PER_UPDATE];

        while (p != NULL) {
            size_t len = strlen(p);

            if (len > 2) {
                char *buffer = (char *)malloc(BUFFER_LEN);
                memset(buffer, 0, BUFFER_LEN);
                strncpy(buffer, p, len);
                buffer[len - 1] = '\0';
                array[elementCount++] = buffer;
            }

            p = strtok(NULL, "\n");
        }

        for (int i = 0; i < elementCount; ++i) {
            processElement(array[i]);
            free(array[i]);
        }
    }
}

void SimSourcePluginStateManager::processElement(char *element)
{
    char *name = strtok(element, "=");
    char *value = strtok(NULL, "=");

    if (value == NULL)
        return;

    char *type = getElementDataType(name[0]);

    if (type != NULL) {
        // SimSourcePluginStateManager::StateManagerInstance()->_logger(LOG_INFO, "%s %s %s", name, value, type);
        simElement el;

        el.name = name;

        if (strncmp(type, "float", sizeof(&type)) == 0) {
            el.type = CONFIG_FLOAT;
            el.value.float_value = atof(value);
            el.length = sizeof(float);
        }
        else if (strncmp(type, "char", sizeof(&type)) == 0) {
            el.type = CONFIG_STRING;
            el.value.string_value = value;
            el.length = strlen(value);
        }
        else if (strncmp(type, "integer", sizeof(&type)) == 0) {
            el.type = CONFIG_INT;
            el.value.int_value = atoi(value);
            el.length = sizeof(int);
        }
        else if (strncmp(type, "bool", sizeof(&type)) == 0) {
            el.type = CONFIG_BOOL;
            el.length = sizeof(int);

            if (strncmp(value, "OFF", sizeof(el.value)) == 0)
                el.value.bool_value = 0;
            else
                el.value.bool_value = 1;
        }
        _enqueueCallback(this, (void *)&el, _callbackArg);
        _processedElementCount++;
    }
}

char *SimSourcePluginStateManager::getElementDataType(char identifier)
{
    switch (identifier) {
    case GAUGE_IDENTIFIER:
        return (char *)"float";
        break;
    case NUMBER_IDENTIFIER:
        return (char *)"float";
        break;
    case INDICATOR_IDENTIFIER:
        return (char *)"bool";
        break;
    case VALUE_IDENTIFIER:
        return (char *)"uint";
        break;
    case ANALOG_IDENTIFIER:
        return (char *)"char";
        break;
    case ROTARY_IDENTIFIER:
        return (char *)"char";
        break;
    case BOOLEAN_IDENTIFIER:
        return (char *)"bool";
        break;
    default:
        return NULL;
    }

    return NULL;
}

void SimSourcePluginStateManager::instanceCloseHandler(uv_handle_t *handle)
{
    if (!_eventLoop->active_handles) {
        uv_stop(_eventLoop);
    }
}

void SimSourcePluginStateManager::ceaseEventing(void)
{
    uv_stop(_eventLoop);
}

void SimSourcePluginStateManager::commenceEventing(EnqueueEventHandler enqueueCallback, void *arg)
{
    _enqueueCallback = enqueueCallback;
    _callbackArg = arg;

    // this is wrong in a number of ways - it needs to be cancel-able being its chief sin
    // TODO: unbreak
    _pluginThread = new std::thread([=] { check_uv(uv_run(_eventLoop, UV_RUN_DEFAULT)); });
}
<commit_msg>more fixing of data element parsing<commit_after>#include <assert.h>
#include <iostream>
#include <memory>
#include <vector>

#include "common/simhubdeviceplugin.h"
#include "main.h"

// -- public C FFI

extern "C" {
int simplug_init(SPHANDLE *plugin_instance, LoggingFunctionCB logger)
{
    *plugin_instance = new SimSourcePluginStateManager(logger);
    return 0;
}

int simplug_config_passthrough(SPHANDLE plugin_instance, void *libconfig_instance)
{
    return static_cast<PluginStateManager *>(plugin_instance)->configPassthrough(static_cast<libconfig::Config *>(libconfig_instance));
}

int simplug_preflight_complete(SPHANDLE plugin_instance)
{
    return static_cast<PluginStateManager *>(plugin_instance)->preflightComplete();
}

void simplug_commence_eventing(SPHANDLE plugin_instance, EnqueueEventHandler enqueue_callback, void *arg)
{
    static_cast<PluginStateManager *>(plugin_instance)->commenceEventing(enqueue_callback, arg);
}

int simplug_deliver_value(SPHANDLE plugin_instance, GenericTLV *value)
{
    return static_cast<PluginStateManager *>(plugin_instance)->deliverValue(value);
}

void simplug_cease_eventing(SPHANDLE plugin_instance)
{
    static_cast<PluginStateManager *>(plugin_instance)->ceaseEventing();
}

void simplug_release(SPHANDLE plugin_instance)
{
    assert(plugin_instance);
    delete static_cast<PluginStateManager *>(plugin_instance);
}
}

// -- internal implementation

SimSourcePluginStateManager *SimSourcePluginStateManager::_StateManagerInstance = NULL;

void SimSourcePluginStateManager::AllocBuffer(uv_handle_t *handle, size_t size, uv_buf_t *buf)
{
    *buf = SimSourcePluginStateManager::StateManagerInstance()->_readBuffer;
}

//! static getter for singleton instance of our class
SimSourcePluginStateManager *SimSourcePluginStateManager::StateManagerInstance(void)
{
    return _StateManagerInstance;
}

SimSourcePluginStateManager::SimSourcePluginStateManager(LoggingFunctionCB logger)
    : PluginStateManager(logger)
{
    // enforce singleton pre-condition

    assert(!_StateManagerInstance);

    _StateManagerInstance = this;
    _processedElementCount = 0;

    if (!(_rawBuffer = (char *)malloc(BUFFER_LEN))) {
        printf("Unable to allocate buffer of size %d", BUFFER_LEN);
    }
    else {
        _readBuffer = ::uv_buf_init(_rawBuffer, BUFFER_LEN);
    }
}

SimSourcePluginStateManager::~SimSourcePluginStateManager(void)
{
    // TODO: enable once shtudown implemented
    if (_pluginThread != NULL) {
        if (_pluginThread->joinable()) {
            ceaseEventing();
            _pluginThread->join();
        }

        delete _pluginThread;
    }

    if (_rawBuffer != NULL)
        free(_rawBuffer);
}

int SimSourcePluginStateManager::preflightComplete(void)
{
    int retVal = PREFLIGHT_OK;
    int port = 8091;

    libconfig::Setting *devicesConfiguraiton = NULL;

    std::string type = "";
    std::string ipAddress = "";

    try {
        devicesConfiguraiton = &_config->lookup("configuration");
    }
    catch (const libconfig::SettingNotFoundException &nfex) {
        _logger(LOG_ERROR, "Config file parse error at %s. Skipping....", nfex.getPath());
    }

    for (libconfig::SettingIterator iter = devicesConfiguraiton->begin(); iter != devicesConfiguraiton->end(); iter++) {

        if (iter->exists("ipAddress")) {
            iter->lookupValue("ipAddress", ipAddress);
        }
        else {
            ipAddress = "127.0.0.1";
        }

        if (iter->exists("port")) {
            iter->lookupValue("port", port);
        }
    }

    struct sockaddr_in req_addr;

    _eventLoop = uv_default_loop();
    check_uv(uv_loop_init(_eventLoop));

    check_uv(uv_tcp_init(_eventLoop, &_tcpClient));
    uv_tcp_keepalive(&_tcpClient, 1, 60);

    uv_ip4_addr(ipAddress.c_str(), port, &req_addr);

    int connectErr = uv_tcp_connect(&_connectReq, &_tcpClient, (struct sockaddr *)&req_addr, &SimSourcePluginStateManager::OnConnect);

    if (connectErr < 0) {
        retVal = PREFLIGHT_FAIL;
    }

    return retVal;
}

void SimSourcePluginStateManager::OnConnect(uv_connect_t *req, int status)
{
    assert(SimSourcePluginStateManager::StateManagerInstance());

    if (status == SIM_CONNECT_NOT_FOUND) {
        SimSourcePluginStateManager::StateManagerInstance()->_logger(LOG_ERROR, "   - Failed to connect to simulator");
    }
    else {
        SimSourcePluginStateManager::StateManagerInstance()->_logger(LOG_INFO, " - Connected to simulator %d", status);
        SimSourcePluginStateManager::StateManagerInstance()->instanceConnectionHandler(req, status);
    }
}

void SimSourcePluginStateManager::OnRead(uv_stream_t *server, ssize_t nread, const uv_buf_t *buf)
{
    SimSourcePluginStateManager::StateManagerInstance()->instanceReadHandler(server, nread, buf);
}

void SimSourcePluginStateManager::OnClose(uv_handle_t *handle)
{
    assert(SimSourcePluginStateManager::StateManagerInstance());
    SimSourcePluginStateManager::StateManagerInstance()->instanceCloseHandler(handle);
}

void SimSourcePluginStateManager::instanceConnectionHandler(uv_connect_t *req, int status)
{
    if (uv_is_readable(req->handle)) {
        uv_read_start(req->handle, &SimSourcePluginStateManager::AllocBuffer, &SimSourcePluginStateManager::OnRead);
    }
    else {
        printf("not readable\n");
    }
}

void SimSourcePluginStateManager::instanceReadHandler(uv_stream_t *server, ssize_t nread, const uv_buf_t *buf)
{
    if (nread > 0) {
        uv_buf_t buffer = uv_buf_init((char *)malloc(nread), nread);
        memcpy(buffer.base, buf->base, nread);
        buffer.base[nread - 1] = '\0';
        processData(buffer.base, nread);
        free(buffer.base);
    }
    else if (nread < 0) {
        if (nread == UV_EOF) {
            SimSourcePluginStateManager::StateManagerInstance()->_logger(LOG_INFO, " - Stopping prepare3d ingest loop");
            ceaseEventing();
        }
        else {
            SimSourcePluginStateManager::StateManagerInstance()->_logger(LOG_INFO, " - %s", uv_strerror(nread));
            uv_close((uv_handle_t *)server, &SimSourcePluginStateManager::OnClose);
        }
    }
}

void SimSourcePluginStateManager::processData(char *data, int len)
{
    if (len > 2) {
        int elementCount = 0;
        char *p = strtok(data, "\n");
        char *array[MAX_ELEMENTS_PER_UPDATE];

        while (p != NULL) {
            size_t len = strlen(p);

            if (len > 2) {
                char *buffer = (char *)malloc(BUFFER_LEN);
                memset(buffer, 0, BUFFER_LEN);
                strncpy(buffer, p, len);
                buffer[len - 1] = '\0';
                array[elementCount++] = buffer;
            }

            p = strtok(NULL, "\n");
        }

        for (int i = 0; i < elementCount; ++i) {
            processElement(array[i]);
            free(array[i]);
        }
    }
}

void SimSourcePluginStateManager::processElement(char *element)
{

    char *name = strtok(element, "=");
    char *value = strtok(NULL, " =");
    name[strlen(name) - 1] = '\0';

    if (value == NULL)
        return;

    char *type = getElementDataType(name[0]);

    if (type != NULL) {

        simElement el;

        el.name = name;

        if (strncmp(type, "float", sizeof(&type)) == 0) {
            el.type = CONFIG_FLOAT;
            el.value.float_value = atof(value);
            el.length = sizeof(float);
        }
        else if (strncmp(type, "char", sizeof(&type)) == 0) {
            el.type = CONFIG_STRING;
            el.value.string_value = value;
            el.length = strlen(value);
        }
        else if (strncmp(type, "integer", sizeof(&type)) == 0) {
            el.type = CONFIG_INT;
            el.value.int_value = atoi(value);
            el.length = sizeof(int);
        }
        else if (strncmp(type, "bool", sizeof(&type)) == 0) {
            el.type = CONFIG_BOOL;
            el.length = sizeof(int);

            if (strncmp(value, "OFF", sizeof(el.value)) == 0)
                el.value.bool_value = 0;
            else
                el.value.bool_value = 1;
        }
        _enqueueCallback(this, (void *)&el, _callbackArg);
        _processedElementCount++;
    }
}

char *SimSourcePluginStateManager::getElementDataType(char identifier)
{
    switch (identifier) {
    case GAUGE_IDENTIFIER:
        return (char *)"float";
        break;
    case NUMBER_IDENTIFIER:
        return (char *)"float";
        break;
    case INDICATOR_IDENTIFIER:
        return (char *)"bool";
        break;
    case VALUE_IDENTIFIER:
        return (char *)"uint";
        break;
    case ANALOG_IDENTIFIER:
        return (char *)"char";
        break;
    case ROTARY_IDENTIFIER:
        return (char *)"char";
        break;
    case BOOLEAN_IDENTIFIER:
        return (char *)"bool";
        break;
    default:
        return NULL;
    }

    return NULL;
}

void SimSourcePluginStateManager::instanceCloseHandler(uv_handle_t *handle)
{
    if (!_eventLoop->active_handles) {
        uv_stop(_eventLoop);
    }
}

void SimSourcePluginStateManager::ceaseEventing(void)
{
    uv_stop(_eventLoop);
}

void SimSourcePluginStateManager::commenceEventing(EnqueueEventHandler enqueueCallback, void *arg)
{
    _enqueueCallback = enqueueCallback;
    _callbackArg = arg;

    // this is wrong in a number of ways - it needs to be cancel-able being its chief sin
    // TODO: unbreak
    _pluginThread = new std::thread([=] { check_uv(uv_run(_eventLoop, UV_RUN_DEFAULT)); });
}
<|endoftext|>