<commit_before>//
// Created by David Li on 6/15/17.
//

#include "SolarSystem.h"

#include <functional>

namespace mapgen {
    SolarSystem::SolarSystem(unsigned int _seed)
        : Generator(_seed) {}

    struct Zone {
        hlt::Location location;
        unsigned short radius;

        Zone(hlt::Location _location, unsigned short _radius)
            : location(_location), radius(_radius) {}
    };

    auto SolarSystem::generate(
        hlt::Map& map,
        unsigned int num_players,
        unsigned int effective_players) -> std::vector<PointOfInterest> {
        assert(effective_players == 2 || effective_players == 4);
        assert(num_players <= effective_players);

        auto spawn_zones = std::vector<Zone>();

        if (effective_players == 2) {
            auto prefer_horizontal = rng() % 2 == 0;

            if (prefer_horizontal) {
                spawn_zones.emplace_back(
                    hlt::Location{
                        static_cast<unsigned short>(map.map_width / 4),
                        static_cast<unsigned short>(map.map_height / 2) },
                    1);
                spawn_zones.emplace_back(
                    hlt::Location{
                        static_cast<unsigned short>(3 * map.map_width / 4),
                        static_cast<unsigned short>(map.map_height / 2) },
                    1);
            } else {
                spawn_zones.emplace_back(
                    hlt::Location{
                        static_cast<unsigned short>(map.map_width / 2),
                        static_cast<unsigned short>(map.map_height / 4) },
                    1);
                spawn_zones.emplace_back(
                    hlt::Location{
                        static_cast<unsigned short>(map.map_width / 2),
                        static_cast<unsigned short>(3 * map.map_height / 4) },
                    1);
            }
        } else {
            spawn_zones.emplace_back(
                hlt::Location{
                    static_cast<unsigned short>(map.map_width / 4),
                    static_cast<unsigned short>(map.map_height / 4) },
                1);
            spawn_zones.emplace_back(
                hlt::Location{
                    static_cast<unsigned short>(3 * map.map_width / 4),
                    static_cast<unsigned short>(map.map_height / 4) },
                1);
            spawn_zones.emplace_back(
                hlt::Location{
                    static_cast<unsigned short>(map.map_width / 4),
                    static_cast<unsigned short>(3 * map.map_height / 4) },
                1);
            spawn_zones.emplace_back(
                hlt::Location{
                    static_cast<unsigned short>(3 * map.map_width / 4),
                    static_cast<unsigned short>(3 * map.map_height / 4) },
                1);
        }

        const auto planets_per_player =
            hlt::GameConstants::get().PLANETS_PER_PLAYER;
        const auto total_planets = effective_players * planets_per_player;
        auto extra_planets = hlt::GameConstants::get().EXTRA_PLANETS;
        const auto center_x = map.map_width / 2;
        const auto center_y = map.map_height / 2;

        const auto max_radius = static_cast<int>(
            std::sqrt(std::min(map.map_width, map.map_height)) / 2);
        auto rand_x_axis = std::bind(
            std::uniform_int_distribution<int>(1, map.map_width / 2 - 1), std::ref(rng));
        auto rand_y_axis = std::bind(
            std::uniform_int_distribution<int>(1, map.map_height / 2 - 1), std::ref(rng));
        auto rand_angle = std::bind(
            std::uniform_real_distribution<double>(0, 2 * M_PI), std::ref(rng));
        auto rand_radius =
            std::bind(std::uniform_int_distribution<int>(3, max_radius), std::ref(rng));
        auto rand_planets_generated =
            std::bind(std::uniform_int_distribution<>(1, std::max(2, planets_per_player / 2)), std::ref(rng));

        // Temporary storage for the planets created in a particular orbit
        auto planets = std::vector<Zone>();

        auto is_ok_location = [&](const hlt::Location& location, int radius) -> bool {
            // I promise this pun was an accident
            for (const auto& zone : spawn_zones) {
                const auto min_distance = zone.radius + radius + 15;
                if (map.get_distance(zone.location, location)
                    <= min_distance) {
                    return false;
                }
            }

            for (const auto& zone : planets) {
                const auto min_distance = zone.radius + radius + 3;
                if (map.get_distance(zone.location, location)
                    <= min_distance) {
                    return false;
                }
            }

            for (const auto& planet : map.planets) {
                const auto min_distance = planet.radius + radius + 15;
                if (map.get_distance(planet.location, location)
                    <= min_distance) {
                    return false;
                }
            }
            return true;
        };

        // Planet in center
        if (extra_planets > 0) {
            extra_planets--;
            const auto big_radius = static_cast<int>(
                std::sqrt(std::min(map.map_width, map.map_height)));
            const auto small_radius = static_cast<int>(
                std::sqrt(std::min(map.map_width, map.map_height) / 1.5));

            for (auto attempt = 0; attempt < 100; attempt++) {
                const auto location = hlt::Location{
                    static_cast<unsigned short>(center_x),
                    static_cast<unsigned short>(center_y),
                };
                const auto radius =
                    std::uniform_int_distribution<>(small_radius, big_radius)(
                        rng);
                if (is_ok_location(location, radius)) {
                    map.planets.emplace_back(
                        location.pos_x,
                        location.pos_y,
                        radius
                    );
                    break;
                }
            }
        }

        // Store the orbits of the planets we generate, so the visualizer
        // can use them
        auto orbits = std::vector<PointOfInterest>();

        auto total_attempts = 0;
        while (map.planets.size() < total_planets && total_attempts < MAX_TOTAL_ATTEMPTS) {
            // Planets to generate per player this iteration
            auto planets_to_generate = rand_planets_generated() * 2;
            if (map.planets.size() + planets_to_generate > total_planets) {
                planets_to_generate = 2;
            }

            for (auto attempt = 0; attempt < 100; attempt++) {
                planets.clear();
                total_attempts++;

                const auto ellipse_x_axis = rand_x_axis();
                const auto ellipse_y_axis = rand_y_axis();
                const auto offset = rand_angle();
                const auto step = 2.0 * M_PI / planets_to_generate;
                const auto radius = rand_radius();

                for (auto planet_index = 0;
                     planet_index < planets_to_generate;
                     planet_index++) {
                    const auto angle = offset + planet_index * step;
                    const auto x = static_cast<unsigned short>(
                        center_x + ellipse_x_axis * std::cos(angle));
                    const auto y = static_cast<unsigned short>(
                        center_y + ellipse_y_axis * std::sin(angle));
                    const auto location = hlt::Location{ x, y };

                    if (!is_ok_location(location, radius)) {
                        goto TRY_AGAIN;
                    }

                    planets.emplace_back(location, radius);
                }

                for (const auto& zone : planets) {
                    map.planets.emplace_back(zone.location.pos_x,
                                             zone.location.pos_y,
                                             zone.radius);
                }

                orbits.push_back({ hlt::Location{
                    static_cast<unsigned short>(center_x),
                    static_cast<unsigned short>(center_y),
                }, ellipse_x_axis, ellipse_y_axis });

                break;

                TRY_AGAIN:;
            }
        }

        while (extra_planets > 0 && total_attempts < MAX_TOTAL_ATTEMPTS) {
            // TODO: can we make this composable?
            total_attempts++;

            const auto choice = std::uniform_int_distribution<>(1, 2)(rng);
            if (choice == 1) {
                // Line of planets down vertical axis
                auto offset = std::uniform_int_distribution<>(max_radius, map.map_height / 2 - max_radius)(rng);
                auto radius = rand_radius();
                auto location1 = hlt::Location{
                    static_cast<unsigned short>(center_x),
                    static_cast<unsigned short>(center_y + offset),
                };
                auto location2 = hlt::Location{
                    static_cast<unsigned short>(center_x),
                    static_cast<unsigned short>(center_y - offset),
                };
                if (is_ok_location(location1, radius) &&
                    is_ok_location(location2, radius)) {
                    // Avoid underflow
                    if (extra_planets <= 2) extra_planets = 0;
                    else extra_planets -= 2;

                    planets.emplace_back(location1, radius);
                    planets.emplace_back(location2, radius);
                }
            }
            else if (choice == 2) {
                // Line of planets down horizontal axis
                auto offset = std::uniform_int_distribution<>(max_radius, map.map_width / 2 - max_radius)(rng);
                auto radius = rand_radius();
                auto location1 = hlt::Location{
                    static_cast<unsigned short>(center_x + offset),
                    static_cast<unsigned short>(center_y),
                };
                auto location2 = hlt::Location{
                    static_cast<unsigned short>(center_x - offset),
                    static_cast<unsigned short>(center_y),
                };
                if (is_ok_location(location1, radius) &&
                    is_ok_location(location2, radius)) {
                    // Avoid underflow
                    if (extra_planets <= 2) extra_planets = 0;
                    else extra_planets -= 2;

                    planets.emplace_back(location1, radius);
                    planets.emplace_back(location2, radius);
                }
            }
            else if (choice == 3) {
                // Planets in corners
            }
        }

        for (hlt::PlayerId player_id = 0; player_id < num_players;
             player_id++) {
            const auto& zone = spawn_zones[player_id];
            for (int i = 0; i < 3; i++) {
                // Spread out ships to make it less likely they'll collide
                // in the start
                map.ships[player_id][i].revive(hlt::Location{
                    zone.location.pos_x,
                    static_cast<unsigned short>(zone.location.pos_y - 2 * (i - 1)),
                });
            }
        }

        return orbits;
    }
}
<commit_msg>Always generate at least 4 planets per orbit (fairer in 4 player)<commit_after>//
// Created by David Li on 6/15/17.
//

#include "SolarSystem.h"

#include <functional>

namespace mapgen {
    SolarSystem::SolarSystem(unsigned int _seed)
        : Generator(_seed) {}

    struct Zone {
        hlt::Location location;
        unsigned short radius;

        Zone(hlt::Location _location, unsigned short _radius)
            : location(_location), radius(_radius) {}
    };

    auto SolarSystem::generate(
        hlt::Map& map,
        unsigned int num_players,
        unsigned int effective_players) -> std::vector<PointOfInterest> {
        assert(effective_players == 2 || effective_players == 4);
        assert(num_players <= effective_players);

        auto spawn_zones = std::vector<Zone>();

        if (effective_players == 2) {
            auto prefer_horizontal = rng() % 2 == 0;

            if (prefer_horizontal) {
                spawn_zones.emplace_back(
                    hlt::Location{
                        static_cast<unsigned short>(map.map_width / 4),
                        static_cast<unsigned short>(map.map_height / 2) },
                    1);
                spawn_zones.emplace_back(
                    hlt::Location{
                        static_cast<unsigned short>(3 * map.map_width / 4),
                        static_cast<unsigned short>(map.map_height / 2) },
                    1);
            } else {
                spawn_zones.emplace_back(
                    hlt::Location{
                        static_cast<unsigned short>(map.map_width / 2),
                        static_cast<unsigned short>(map.map_height / 4) },
                    1);
                spawn_zones.emplace_back(
                    hlt::Location{
                        static_cast<unsigned short>(map.map_width / 2),
                        static_cast<unsigned short>(3 * map.map_height / 4) },
                    1);
            }
        } else {
            spawn_zones.emplace_back(
                hlt::Location{
                    static_cast<unsigned short>(map.map_width / 4),
                    static_cast<unsigned short>(map.map_height / 4) },
                1);
            spawn_zones.emplace_back(
                hlt::Location{
                    static_cast<unsigned short>(3 * map.map_width / 4),
                    static_cast<unsigned short>(map.map_height / 4) },
                1);
            spawn_zones.emplace_back(
                hlt::Location{
                    static_cast<unsigned short>(map.map_width / 4),
                    static_cast<unsigned short>(3 * map.map_height / 4) },
                1);
            spawn_zones.emplace_back(
                hlt::Location{
                    static_cast<unsigned short>(3 * map.map_width / 4),
                    static_cast<unsigned short>(3 * map.map_height / 4) },
                1);
        }

        const auto planets_per_player =
            hlt::GameConstants::get().PLANETS_PER_PLAYER;
        const auto total_planets = effective_players * planets_per_player;
        auto extra_planets = hlt::GameConstants::get().EXTRA_PLANETS;
        const auto center_x = map.map_width / 2;
        const auto center_y = map.map_height / 2;

        const auto max_radius = static_cast<int>(
            std::sqrt(std::min(map.map_width, map.map_height)) / 2);
        auto rand_x_axis = std::bind(
            std::uniform_int_distribution<int>(1, map.map_width / 2 - 1), std::ref(rng));
        auto rand_y_axis = std::bind(
            std::uniform_int_distribution<int>(1, map.map_height / 2 - 1), std::ref(rng));
        auto rand_angle = std::bind(
            std::uniform_real_distribution<double>(0, 2 * M_PI), std::ref(rng));
        auto rand_radius =
            std::bind(std::uniform_int_distribution<int>(3, max_radius), std::ref(rng));
        auto rand_planets_generated =
            std::bind(std::uniform_int_distribution<>(4, std::max(2, planets_per_player / 2)), std::ref(rng));

        // Temporary storage for the planets created in a particular orbit
        auto planets = std::vector<Zone>();

        auto is_ok_location = [&](const hlt::Location& location, int radius) -> bool {
            // I promise this pun was an accident
            for (const auto& zone : spawn_zones) {
                const auto min_distance = zone.radius + radius + 15;
                if (map.get_distance(zone.location, location)
                    <= min_distance) {
                    return false;
                }
            }

            for (const auto& zone : planets) {
                const auto min_distance = zone.radius + radius + 3;
                if (map.get_distance(zone.location, location)
                    <= min_distance) {
                    return false;
                }
            }

            for (const auto& planet : map.planets) {
                const auto min_distance = planet.radius + radius + 15;
                if (map.get_distance(planet.location, location)
                    <= min_distance) {
                    return false;
                }
            }
            return true;
        };

        // Planet in center
        if (extra_planets > 0) {
            extra_planets--;
            const auto big_radius = static_cast<int>(
                std::sqrt(std::min(map.map_width, map.map_height)));
            const auto small_radius = static_cast<int>(
                std::sqrt(std::min(map.map_width, map.map_height) / 1.5));

            for (auto attempt = 0; attempt < 100; attempt++) {
                const auto location = hlt::Location{
                    static_cast<unsigned short>(center_x),
                    static_cast<unsigned short>(center_y),
                };
                const auto radius =
                    std::uniform_int_distribution<>(small_radius, big_radius)(
                        rng);
                if (is_ok_location(location, radius)) {
                    map.planets.emplace_back(
                        location.pos_x,
                        location.pos_y,
                        radius
                    );
                    break;
                }
            }
        }

        // Store the orbits of the planets we generate, so the visualizer
        // can use them
        auto orbits = std::vector<PointOfInterest>();

        auto total_attempts = 0;
        while (map.planets.size() < total_planets && total_attempts < MAX_TOTAL_ATTEMPTS) {
            // Planets to generate per player this iteration
            auto planets_to_generate = rand_planets_generated() * 2;
            if (map.planets.size() + planets_to_generate > total_planets) {
                planets_to_generate = 4;
            }

            for (auto attempt = 0; attempt < 100; attempt++) {
                planets.clear();
                total_attempts++;

                const auto ellipse_x_axis = rand_x_axis();
                const auto ellipse_y_axis = rand_y_axis();
                const auto offset = rand_angle();
                const auto step = 2.0 * M_PI / planets_to_generate;
                const auto radius = rand_radius();

                for (auto planet_index = 0;
                     planet_index < planets_to_generate;
                     planet_index++) {
                    const auto angle = offset + planet_index * step;
                    const auto x = static_cast<unsigned short>(
                        center_x + ellipse_x_axis * std::cos(angle));
                    const auto y = static_cast<unsigned short>(
                        center_y + ellipse_y_axis * std::sin(angle));
                    const auto location = hlt::Location{ x, y };

                    if (!is_ok_location(location, radius)) {
                        goto TRY_AGAIN;
                    }

                    planets.emplace_back(location, radius);
                }

                for (const auto& zone : planets) {
                    map.planets.emplace_back(zone.location.pos_x,
                                             zone.location.pos_y,
                                             zone.radius);
                }

                orbits.push_back({ hlt::Location{
                    static_cast<unsigned short>(center_x),
                    static_cast<unsigned short>(center_y),
                }, ellipse_x_axis, ellipse_y_axis });

                break;

                TRY_AGAIN:;
            }
        }

        while (extra_planets > 0 && total_attempts < MAX_TOTAL_ATTEMPTS) {
            // TODO: can we make this composable?
            total_attempts++;

            const auto choice = std::uniform_int_distribution<>(1, 2)(rng);
            if (choice == 1) {
                // Line of planets down vertical axis
                auto offset = std::uniform_int_distribution<>(max_radius, map.map_height / 2 - max_radius)(rng);
                auto radius = rand_radius();
                auto location1 = hlt::Location{
                    static_cast<unsigned short>(center_x),
                    static_cast<unsigned short>(center_y + offset),
                };
                auto location2 = hlt::Location{
                    static_cast<unsigned short>(center_x),
                    static_cast<unsigned short>(center_y - offset),
                };
                if (is_ok_location(location1, radius) &&
                    is_ok_location(location2, radius)) {
                    // Avoid underflow
                    if (extra_planets <= 2) extra_planets = 0;
                    else extra_planets -= 2;

                    planets.emplace_back(location1, radius);
                    planets.emplace_back(location2, radius);
                }
            }
            else if (choice == 2) {
                // Line of planets down horizontal axis
                auto offset = std::uniform_int_distribution<>(max_radius, map.map_width / 2 - max_radius)(rng);
                auto radius = rand_radius();
                auto location1 = hlt::Location{
                    static_cast<unsigned short>(center_x + offset),
                    static_cast<unsigned short>(center_y),
                };
                auto location2 = hlt::Location{
                    static_cast<unsigned short>(center_x - offset),
                    static_cast<unsigned short>(center_y),
                };
                if (is_ok_location(location1, radius) &&
                    is_ok_location(location2, radius)) {
                    // Avoid underflow
                    if (extra_planets <= 2) extra_planets = 0;
                    else extra_planets -= 2;

                    planets.emplace_back(location1, radius);
                    planets.emplace_back(location2, radius);
                }
            }
            else if (choice == 3) {
                // Planets in corners
            }
        }

        for (hlt::PlayerId player_id = 0; player_id < num_players;
             player_id++) {
            const auto& zone = spawn_zones[player_id];
            for (int i = 0; i < 3; i++) {
                // Spread out ships to make it less likely they'll collide
                // in the start
                map.ships[player_id][i].revive(hlt::Location{
                    zone.location.pos_x,
                    static_cast<unsigned short>(zone.location.pos_y - 2 * (i - 1)),
                });
            }
        }

        return orbits;
    }
}
<|endoftext|>


<commit_before>#include <ros/ros.h>
#include <ros/console.h>
#include <dynamic_reconfigure/server.h>
#include <avoiding_point_generator/parameterConfig.h>
#include <vector>
#include <string>
#include <algorithm> //std::copy
#include <iterator> //std::back_inserter

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Point.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/UInt16MultiArray.h>
#include <consai_msgs/AIStatus.h>

#include "transformation.h"

// ロボット位置情報と衝突回避コンフィグを保管するグローバル空間
namespace GlobalInfo {
    std::vector<geometry_msgs::Point> targetPoints(12);
    std::vector<geometry_msgs::Point> friendPoints(12);
    std::vector<geometry_msgs::Point> enemyPoints(12);
    std::vector<int> friendIDs;
    std::vector<int> enemyIDs;
    geometry_msgs::Point ballPoint;

    // Obstacle Avoidance Config
    struct ObstacleAvoidanceConfig{
        double detectRange = 0.5;
        double avoidRange = 0.5;
        double startDetectionPos = 0.0;
        double avoidHysteresis = 0.18;
    };

    ObstacleAvoidanceConfig OAConfig;

    void callbackTargetPoint(const geometry_msgs::PoseStampedConstPtr &msg, const int id){
        targetPoints[id] = msg->pose.position;
    }

    void callbackFriendPoint(const nav_msgs::OdometryConstPtr &msg, const int id){
        friendPoints[id] = msg->pose.pose.position;
    }

    void callbackEnemyPoint(const nav_msgs::OdometryConstPtr &msg, const int id){
        enemyPoints[id] = msg->pose.pose.position;
    }

    void callbackFriendIDs(const std_msgs::UInt16MultiArrayConstPtr& msg){
        friendIDs.clear();
        std::copy(msg->data.begin(), msg->data.end(), std::back_inserter(friendIDs));
    }

    void callbackEnemyIDs(const std_msgs::UInt16MultiArrayConstPtr& msg){
        enemyIDs.clear();
        std::copy(msg->data.begin(), msg->data.end(), std::back_inserter(enemyIDs));
    }

    void callbackBallPoint(const nav_msgs::OdometryConstPtr &msg){
        ballPoint = msg->pose.pose.position;
    }

    void callbackReconfigure(avoiding_point_generator::parameterConfig &config, uint32_t level){
        OAConfig.detectRange = config.detectRange;
        OAConfig.avoidRange = config.avoidRange;
        OAConfig.startDetectionPos = config.startDetectionPos;
        OAConfig.avoidHysteresis = config.avoidHysteresis;
    }
}

class ObstacleAvoidingPointGenerator{
    public:
        ObstacleAvoidingPointGenerator(){
        }

        void updateAvoidingPoint();

        geometry_msgs::Point getAvoidingPoint(){
            return mAvoidingPoint;
        }

        void setTargetPoint(const geometry_msgs::Point &point){
            mTargetPoint = point;
        }

        void setRobotPoint(const geometry_msgs::Point &point){
            mRobotPoint = point;
        }

        void callbackAIStatus(const consai_msgs::AIStatus &msg){
            mAIStatus = msg;
        }

    private:
        geometry_msgs::Point mTargetPoint;
        geometry_msgs::Point mRobotPoint;
        geometry_msgs::Point mAvoidingPoint;
        consai_msgs::AIStatus mAIStatus;
        bool mAvoidPointIsUpperImag;

        struct OAParameter{
            double trAvoidReal;
            double trAvoidUpperImag;
            double trAvoidLowerImag;
            double trGoalPosReal;
            bool avoiding;
            OAParameter(){
                trAvoidReal = 0;
                trAvoidUpperImag = 0;
                trAvoidLowerImag = 0;
                trGoalPosReal = 0;
                avoiding = false;
            }
        };

        OAParameter detectObstacle(const Transformation &trans,
                const geometry_msgs::Point &point, const OAParameter &parameter);
        OAParameter detectObstacleOverlap(const Transformation &trans,
                const geometry_msgs::Point &point, const OAParameter &parameter);
};

void ObstacleAvoidingPointGenerator::updateAvoidingPoint(){
    // 障害物の横に回避ポイントを作成する
    // 障害物が横並びの時は、大幅に避けるポイントを作成する

    // 自分の位置から目標位置までの座標系を作成
    Complex startPos = Tool::toComplex(mRobotPoint);
    Complex goalPos = Tool::toComplex(mTargetPoint);
    double angleToGoal = Tool::getAngle(goalPos - startPos);

    Transformation trans(startPos, angleToGoal);
    Complex trGoalPos = trans.transform(goalPos);

    // 自分に最も近い障害物の発見し、その回避位置を生成
    OAParameter parameter;
    parameter.trAvoidReal = trGoalPos.real();
    parameter.trGoalPosReal = trGoalPos.real();

    // 敵ロボット
    for(int id : GlobalInfo::enemyIDs){
        geometry_msgs::Point obstacle = GlobalInfo::enemyPoints[id];
        parameter = detectObstacle(trans, obstacle, parameter);
    }
    // 味方ロボット
    for(int id : GlobalInfo::friendIDs){
        geometry_msgs::Point obstacle = GlobalInfo::friendPoints[id];
        parameter = detectObstacle(trans, obstacle, parameter);
    }
    // ボール回避は任意
    if(mAIStatus.avoidBall){
        parameter = detectObstacle(trans, GlobalInfo::ballPoint, parameter);
    }

    // 横並びロボット回避位置を生成
    // 障害物がなければ計算省略
    if(parameter.avoiding){
        // 横並び計算漏れを防ぐため計算をループさせる(ループ回数はテキトー)
        for(int loop=0; loop < 7; loop++){
            double old_upperImag = parameter.trAvoidUpperImag;
            double old_lowerImag = parameter.trAvoidLowerImag;

            // 敵ロボット
            for(int id : GlobalInfo::enemyIDs){
                geometry_msgs::Point obstacle = GlobalInfo::enemyPoints[id];
                parameter = detectObstacleOverlap(trans, obstacle, parameter);
            }
            // 見方ロボット
            for(int id : GlobalInfo::friendIDs){
                geometry_msgs::Point obstacle = GlobalInfo::friendPoints[id];
                parameter = detectObstacleOverlap(trans, obstacle, parameter);
            }
            // ボール回避は任意
            if(mAIStatus.avoidBall){
                parameter = detectObstacleOverlap(trans, GlobalInfo::ballPoint, parameter);
            }

            // 回避位置がほとんど更新されなかったらループを抜ける
            if(Tool::isEqual(old_upperImag, parameter.trAvoidUpperImag) &&
                    Tool::isEqual(old_lowerImag, parameter.trAvoidLowerImag)){
                break;
            }
        }

        // 近い方のImagでAvoidingPointを生成する
        Complex trAvoidPos = trGoalPos;
        double upperSize = fabs(parameter.trAvoidUpperImag);
        double lowerSize = fabs(parameter.trAvoidLowerImag);

        // 左右の回避位置生成にヒステリシスをもたせる
        if(mAvoidPointIsUpperImag == true){
            if(lowerSize < upperSize - GlobalInfo::OAConfig.avoidHysteresis){
                mAvoidPointIsUpperImag = false;
            }
        }else{
            if(upperSize < lowerSize - GlobalInfo::OAConfig.avoidHysteresis){
                mAvoidPointIsUpperImag = true;
            }
        }
        if(mAvoidPointIsUpperImag == true){
            trAvoidPos = Complex(parameter.trAvoidReal, parameter.trAvoidUpperImag);
        }else{
            trAvoidPos = Complex(parameter.trAvoidReal, parameter.trAvoidLowerImag);
        }

        // 座標系を元に戻して終了
        Complex avoidPos = trans.invertedTransform(trAvoidPos);
        mAvoidingPoint.x = avoidPos.real();
        mAvoidingPoint.y = avoidPos.imag();
    }else{
        // 障害物がない場合は、targetPosをavoidingPointに設定する
        mAvoidingPoint = mTargetPoint;
    }
}

ObstacleAvoidingPointGenerator::OAParameter ObstacleAvoidingPointGenerator::detectObstacle(
        const Transformation &trans,
        const geometry_msgs::Point &point, const OAParameter &parameter){

    OAParameter output = parameter;

    Complex obstaclePos = Tool::toComplex(point);
    Complex trObstPos = trans.transform(obstaclePos);

    // 自分と移動目標位置を結んだ直線上にいる障害物を見つける
    // 一番近い障害物を見つけるため
    // parameterのtrAvoidRealを障害物のrealで上書きしていく
    if(trObstPos.real() > GlobalInfo::OAConfig.startDetectionPos
            && trObstPos.real() < parameter.trAvoidReal
            && fabs(trObstPos.imag()) < GlobalInfo::OAConfig.detectRange){

        // 避けれる上下位置を計算
        double upperImag = trObstPos.imag() + GlobalInfo::OAConfig.avoidRange;
        double lowerImag = trObstPos.imag() - GlobalInfo::OAConfig.avoidRange;

        output.trAvoidReal = trObstPos.real();
        output.trAvoidUpperImag = upperImag;
        output.trAvoidLowerImag = lowerImag;
        output.avoiding = true;
    }

    return output;
}

ObstacleAvoidingPointGenerator::OAParameter ObstacleAvoidingPointGenerator::detectObstacleOverlap(
        const Transformation &trans,
        const geometry_msgs::Point &point, const OAParameter &parameter){

    OAParameter output = parameter;

    Complex obstaclePos = Tool::toComplex(point);
    Complex trObstPos = trans.transform(obstaclePos);

    // 回避範囲(upper <-> lower)の重なりを見つけ、最端の回避位置を生成する
    if(trObstPos.real() > GlobalInfo::OAConfig.startDetectionPos
            && trObstPos.real() < parameter.trGoalPosReal){
        double upperImag = trObstPos.imag() + GlobalInfo::OAConfig.avoidRange;
        double lowerImag = trObstPos.imag() - GlobalInfo::OAConfig.avoidRange;

        if(upperImag > parameter.trAvoidLowerImag
                && upperImag < parameter.trAvoidUpperImag
                && lowerImag < parameter.trAvoidLowerImag){

            output.trAvoidLowerImag = lowerImag;
        }else if(lowerImag > parameter.trAvoidLowerImag
                && lowerImag < parameter.trAvoidUpperImag
                && upperImag > parameter.trAvoidUpperImag){

            output.trAvoidUpperImag = upperImag;
        }
    }

    return output;
}

int main(int argc, char **argv){
    ros::init(argc, argv, "avoiding_point_generator");
    ros::NodeHandle nh;
    ros::Rate r(60);

    // Dynamic Reconfigureの設定
    dynamic_reconfigure::Server<avoiding_point_generator::parameterConfig> reconfigure_server;
    dynamic_reconfigure::Server<avoiding_point_generator::parameterConfig>::CallbackType f;

    f = boost::bind(&GlobalInfo::callbackReconfigure, _1, _2);
    reconfigure_server.setCallback(f);

    std::vector<ObstacleAvoidingPointGenerator> generator(12);

    // // 起動時のnamespaceを取得
    // std::string ai_name = "/";
    // nh.getParam("ai_name", ai_name);

    // Subscriberを作成
    ros::Subscriber subFriendIDs = nh.subscribe("existing_friends_id", 100,
            GlobalInfo::callbackFriendIDs);
    ros::Subscriber subEnemyIDs = nh.subscribe("existing_enemies_id", 100,
            GlobalInfo::callbackEnemyIDs);
    ros::Subscriber subBallPoint = nh.subscribe("ball_observer/estimation", 100,
            GlobalInfo::callbackBallPoint);

    std::vector<ros::Subscriber> subs_targetPoint;
    std::vector<ros::Subscriber> subs_friendPoint;
    std::vector<ros::Subscriber> subs_enemyPoint;
    std::vector<ros::Subscriber> subs_AIStatus;

    // Publisherを作成
    std::vector<ros::Publisher> pubs_avoidingPoint;

    for(int i=0; i< 12; i++){
        std::string topicName;

        // callbackに引数を与えるため、boost::bindを使用
        topicName = "robot_" + std::to_string(i) + "/move_base_simple/goal";
        ros::Subscriber sub = nh.subscribe<geometry_msgs::PoseStamped>(
                topicName.c_str(), 100,
                boost::bind(GlobalInfo::callbackTargetPoint, _1, i));
        subs_targetPoint.push_back(sub);

        topicName = "robot_" + std::to_string(i) + "/odom";
        sub = nh.subscribe<nav_msgs::Odometry>(
                topicName.c_str(), 100,
                boost::bind(GlobalInfo::callbackFriendPoint, _1, i));
        subs_friendPoint.push_back(sub);

        topicName = "enemy_" + std::to_string(i) + "/odom";
        sub = nh.subscribe<nav_msgs::Odometry>(
                topicName.c_str(), 100,
                boost::bind(GlobalInfo::callbackEnemyPoint, _1, i));
        subs_enemyPoint.push_back(sub);

        topicName = "robot_" + std::to_string(i) + "/ai_status";
        sub = nh.subscribe(
                topicName.c_str(), 100,
                &ObstacleAvoidingPointGenerator::callbackAIStatus,
                &generator[i]);
        subs_AIStatus.push_back(sub);

        topicName = "robot_" + std::to_string(i) + "/avoiding_point";
        ros::Publisher pub = nh.advertise<geometry_msgs::Point>(topicName, 100);
        pubs_avoidingPoint.push_back(pub);
    }

    while (ros::ok()){
        // 存在している味方ロボットのavoidingPointを計算する
        for(int id : GlobalInfo::friendIDs){
            generator[id].setRobotPoint(GlobalInfo::friendPoints[id]);
            generator[id].setTargetPoint(GlobalInfo::targetPoints[id]);
            generator[id].updateAvoidingPoint();

            pubs_avoidingPoint[id].publish(generator[id].getAvoidingPoint());
        }

        ros::spinOnce();
        r.sleep();
    }

    return 0;
}
<commit_msg>敵ディフェンスエリアを回避する<commit_after>#include <ros/ros.h>
#include <ros/console.h>
#include <dynamic_reconfigure/server.h>
#include <avoiding_point_generator/parameterConfig.h>
#include <vector>
#include <string>
#include <algorithm> //std::copy
#include <iterator> //std::back_inserter

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Point.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/UInt16MultiArray.h>
#include <consai_msgs/AIStatus.h>

#include "transformation.h"

// ロボット位置情報と衝突回避コンフィグを保管するグローバル空間
namespace GlobalInfo {
    std::vector<geometry_msgs::Point> targetPoints(12);
    std::vector<geometry_msgs::Point> friendPoints(12);
    std::vector<geometry_msgs::Point> enemyPoints(12);
    std::vector<int> friendIDs;
    std::vector<int> enemyIDs;
    geometry_msgs::Point ballPoint;
    geometry_msgs::Point friendDefencePoint;
    geometry_msgs::Point enemyUpperDefencePoint;
    geometry_msgs::Point enemyLowerDefencePoint;

    // Obstacle Avoidance Config
    struct ObstacleAvoidanceConfig{
        double detectRange = 0.5;
        double avoidRange = 0.5;
        double startDetectionPos = 0.0;
        double avoidHysteresis = 0.18;
        double detectDefenceRange = 0.6;
        double avoidDefenceRange = 0.6;
    };

    ObstacleAvoidanceConfig OAConfig;

    void callbackTargetPoint(const geometry_msgs::PoseStampedConstPtr &msg, const int id){
        targetPoints[id] = msg->pose.position;
    }

    void callbackFriendPoint(const nav_msgs::OdometryConstPtr &msg, const int id){
        friendPoints[id] = msg->pose.pose.position;
    }

    void callbackEnemyPoint(const nav_msgs::OdometryConstPtr &msg, const int id){
        enemyPoints[id] = msg->pose.pose.position;
    }

    void callbackFriendIDs(const std_msgs::UInt16MultiArrayConstPtr& msg){
        friendIDs.clear();
        std::copy(msg->data.begin(), msg->data.end(), std::back_inserter(friendIDs));
    }

    void callbackEnemyIDs(const std_msgs::UInt16MultiArrayConstPtr& msg){
        enemyIDs.clear();
        std::copy(msg->data.begin(), msg->data.end(), std::back_inserter(enemyIDs));
    }

    void callbackBallPoint(const nav_msgs::OdometryConstPtr &msg){
        ballPoint = msg->pose.pose.position;
    }

    void callbackReconfigure(avoiding_point_generator::parameterConfig &config, uint32_t level){
        OAConfig.detectRange = config.detectRange;
        OAConfig.avoidRange = config.avoidRange;
        OAConfig.startDetectionPos = config.startDetectionPos;
        OAConfig.avoidHysteresis = config.avoidHysteresis;
    }
}

class ObstacleAvoidingPointGenerator{
    public:
        ObstacleAvoidingPointGenerator(){
        }

        void updateAvoidingPoint();

        geometry_msgs::Point getAvoidingPoint(){
            return mAvoidingPoint;
        }

        void setTargetPoint(const geometry_msgs::Point &point){
            mTargetPoint = point;
        }

        void setRobotPoint(const geometry_msgs::Point &point){
            mRobotPoint = point;
        }

        void callbackAIStatus(const consai_msgs::AIStatus &msg){
            mAIStatus = msg;
        }

    private:
        geometry_msgs::Point mTargetPoint;
        geometry_msgs::Point mRobotPoint;
        geometry_msgs::Point mAvoidingPoint;
        consai_msgs::AIStatus mAIStatus;
        bool mAvoidPointIsUpperImag;

        struct OAParameter{
            double trAvoidReal;
            double trAvoidUpperImag;
            double trAvoidLowerImag;
            double trGoalPosReal;
            bool avoiding;
            OAParameter(){
                trAvoidReal = 0;
                trAvoidUpperImag = 0;
                trAvoidLowerImag = 0;
                trGoalPosReal = 0;
                avoiding = false;
            }
        };

        OAParameter detectObstacle(const Transformation &trans,
                const geometry_msgs::Point &point, const OAParameter &parameter,
                double detectRange = GlobalInfo::OAConfig.detectRange,
                double avoidRange = GlobalInfo::OAConfig.avoidRange);
        OAParameter detectObstacleOverlap(const Transformation &trans,
                const geometry_msgs::Point &point, const OAParameter &parameter,
                double avoidRange = GlobalInfo::OAConfig.avoidRange);
};

void ObstacleAvoidingPointGenerator::updateAvoidingPoint(){
    // 障害物の横に回避ポイントを作成する
    // 障害物が横並びの時は、大幅に避けるポイントを作成する

    // 自分の位置から目標位置までの座標系を作成
    Complex startPos = Tool::toComplex(mRobotPoint);
    Complex goalPos = Tool::toComplex(mTargetPoint);
    double angleToGoal = Tool::getAngle(goalPos - startPos);

    Transformation trans(startPos, angleToGoal);
    Complex trGoalPos = trans.transform(goalPos);

    bool avoidDefence = true;

    // 自分に最も近い障害物の発見し、その回避位置を生成
    OAParameter parameter;
    parameter.trAvoidReal = trGoalPos.real();
    parameter.trGoalPosReal = trGoalPos.real();

    // 敵ロボット
    for(int id : GlobalInfo::enemyIDs){
        geometry_msgs::Point obstacle = GlobalInfo::enemyPoints[id];
        parameter = detectObstacle(trans, obstacle, parameter);
    }
    // 味方ロボット
    for(int id : GlobalInfo::friendIDs){
        geometry_msgs::Point obstacle = GlobalInfo::friendPoints[id];
        parameter = detectObstacle(trans, obstacle, parameter);
    }
    // ボール回避は任意
    if(mAIStatus.avoidBall){
        parameter = detectObstacle(trans, GlobalInfo::ballPoint, parameter);
    }

    // ディフェンスエリア回避は任意
    if(avoidDefence){
        parameter = detectObstacle(trans, GlobalInfo::enemyUpperDefencePoint,
                parameter, GlobalInfo::OAConfig.detectDefenceRange,
                GlobalInfo::OAConfig.avoidDefenceRange);
        parameter = detectObstacle(trans, GlobalInfo::enemyLowerDefencePoint,
                parameter, GlobalInfo::OAConfig.detectDefenceRange,
                GlobalInfo::OAConfig.avoidDefenceRange);
    }

    // 横並びロボット回避位置を生成
    // 障害物がなければ計算省略
    if(parameter.avoiding){
        // 横並び計算漏れを防ぐため計算をループさせる(ループ回数はテキトー)
        for(int loop=0; loop < 7; loop++){
            double old_upperImag = parameter.trAvoidUpperImag;
            double old_lowerImag = parameter.trAvoidLowerImag;

            // 敵ロボット
            for(int id : GlobalInfo::enemyIDs){
                geometry_msgs::Point obstacle = GlobalInfo::enemyPoints[id];
                parameter = detectObstacleOverlap(trans, obstacle, parameter);
            }
            // 見方ロボット
            for(int id : GlobalInfo::friendIDs){
                geometry_msgs::Point obstacle = GlobalInfo::friendPoints[id];
                parameter = detectObstacleOverlap(trans, obstacle, parameter);
            }
            // ボール回避は任意
            if(mAIStatus.avoidBall){
                parameter = detectObstacleOverlap(trans, GlobalInfo::ballPoint, parameter);
            }

            // ディフェンスエリア回避は任意
            if(avoidDefence){
                parameter = detectObstacleOverlap(trans, GlobalInfo::enemyUpperDefencePoint,
                        parameter, GlobalInfo::OAConfig.avoidDefenceRange);
                parameter = detectObstacleOverlap(trans, GlobalInfo::enemyLowerDefencePoint,
                        parameter, GlobalInfo::OAConfig.avoidDefenceRange);
            }

            // 回避位置がほとんど更新されなかったらループを抜ける
            if(Tool::isEqual(old_upperImag, parameter.trAvoidUpperImag) &&
                    Tool::isEqual(old_lowerImag, parameter.trAvoidLowerImag)){
                break;
            }
        }

        // 近い方のImagでAvoidingPointを生成する
        Complex trAvoidPos = trGoalPos;
        double upperSize = fabs(parameter.trAvoidUpperImag);
        double lowerSize = fabs(parameter.trAvoidLowerImag);

        // 左右の回避位置生成にヒステリシスをもたせる
        if(mAvoidPointIsUpperImag == true){
            if(lowerSize < upperSize - GlobalInfo::OAConfig.avoidHysteresis){
                mAvoidPointIsUpperImag = false;
            }
        }else{
            if(upperSize < lowerSize - GlobalInfo::OAConfig.avoidHysteresis){
                mAvoidPointIsUpperImag = true;
            }
        }
        if(mAvoidPointIsUpperImag == true){
            trAvoidPos = Complex(parameter.trAvoidReal, parameter.trAvoidUpperImag);
        }else{
            trAvoidPos = Complex(parameter.trAvoidReal, parameter.trAvoidLowerImag);
        }

        // 座標系を元に戻して終了
        Complex avoidPos = trans.invertedTransform(trAvoidPos);
        mAvoidingPoint.x = avoidPos.real();
        mAvoidingPoint.y = avoidPos.imag();
    }else{
        // 障害物がない場合は、targetPosをavoidingPointに設定する
        mAvoidingPoint = mTargetPoint;
    }
}

ObstacleAvoidingPointGenerator::OAParameter ObstacleAvoidingPointGenerator::detectObstacle(
        const Transformation &trans,
        const geometry_msgs::Point &point, const OAParameter &parameter,
        double detectRange, double avoidRange){

    OAParameter output = parameter;

    Complex obstaclePos = Tool::toComplex(point);
    Complex trObstPos = trans.transform(obstaclePos);

    // 自分と移動目標位置を結んだ直線上にいる障害物を見つける
    // 一番近い障害物を見つけるため
    // parameterのtrAvoidRealを障害物のrealで上書きしていく
    if(trObstPos.real() > GlobalInfo::OAConfig.startDetectionPos
            && trObstPos.real() < parameter.trAvoidReal
            && fabs(trObstPos.imag()) < detectRange){

        // 避けれる上下位置を計算
        double upperImag = trObstPos.imag() + avoidRange;
        double lowerImag = trObstPos.imag() - avoidRange;

        output.trAvoidReal = trObstPos.real();
        output.trAvoidUpperImag = upperImag;
        output.trAvoidLowerImag = lowerImag;
        output.avoiding = true;
    }

    return output;
}

ObstacleAvoidingPointGenerator::OAParameter ObstacleAvoidingPointGenerator::detectObstacleOverlap(
        const Transformation &trans,
        const geometry_msgs::Point &point, const OAParameter &parameter,
        double avoidRange){

    OAParameter output = parameter;

    Complex obstaclePos = Tool::toComplex(point);
    Complex trObstPos = trans.transform(obstaclePos);

    // 回避範囲(upper <-> lower)の重なりを見つけ、最端の回避位置を生成する
    if(trObstPos.real() > GlobalInfo::OAConfig.startDetectionPos
            && trObstPos.real() < parameter.trGoalPosReal){
        double upperImag = trObstPos.imag() + avoidRange;
        double lowerImag = trObstPos.imag() - avoidRange;

        if(upperImag > parameter.trAvoidLowerImag
                && upperImag < parameter.trAvoidUpperImag
                && lowerImag < parameter.trAvoidLowerImag){

            output.trAvoidLowerImag = lowerImag;
        }else if(lowerImag > parameter.trAvoidLowerImag
                && lowerImag < parameter.trAvoidUpperImag
                && upperImag > parameter.trAvoidUpperImag){

            output.trAvoidUpperImag = upperImag;
        }
    }

    return output;
}

int main(int argc, char **argv){
    ros::init(argc, argv, "avoiding_point_generator");
    ros::NodeHandle nh;
    ros::Rate r(60);

    // Dynamic Reconfigureの設定
    dynamic_reconfigure::Server<avoiding_point_generator::parameterConfig> reconfigure_server;
    dynamic_reconfigure::Server<avoiding_point_generator::parameterConfig>::CallbackType f;

    f = boost::bind(&GlobalInfo::callbackReconfigure, _1, _2);
    reconfigure_server.setCallback(f);

    std::vector<ObstacleAvoidingPointGenerator> generator(12);

    // GlobalInfo初期化
    GlobalInfo::enemyUpperDefencePoint.x = 4.0;
    GlobalInfo::enemyUpperDefencePoint.y = 0.5;
    GlobalInfo::enemyLowerDefencePoint.x = 4.0;
    GlobalInfo::enemyLowerDefencePoint.y = -0.5;

    // Subscriberを作成
    ros::Subscriber subFriendIDs = nh.subscribe("existing_friends_id", 100,
            GlobalInfo::callbackFriendIDs);
    ros::Subscriber subEnemyIDs = nh.subscribe("existing_enemies_id", 100,
            GlobalInfo::callbackEnemyIDs);
    ros::Subscriber subBallPoint = nh.subscribe("ball_observer/estimation", 100,
            GlobalInfo::callbackBallPoint);

    std::vector<ros::Subscriber> subs_targetPoint;
    std::vector<ros::Subscriber> subs_friendPoint;
    std::vector<ros::Subscriber> subs_enemyPoint;
    std::vector<ros::Subscriber> subs_AIStatus;

    // Publisherを作成
    std::vector<ros::Publisher> pubs_avoidingPoint;

    for(int i=0; i< 12; i++){
        std::string topicName;

        // callbackに引数を与えるため、boost::bindを使用
        topicName = "robot_" + std::to_string(i) + "/move_base_simple/goal";
        ros::Subscriber sub = nh.subscribe<geometry_msgs::PoseStamped>(
                topicName.c_str(), 100,
                boost::bind(GlobalInfo::callbackTargetPoint, _1, i));
        subs_targetPoint.push_back(sub);

        topicName = "robot_" + std::to_string(i) + "/odom";
        sub = nh.subscribe<nav_msgs::Odometry>(
                topicName.c_str(), 100,
                boost::bind(GlobalInfo::callbackFriendPoint, _1, i));
        subs_friendPoint.push_back(sub);

        topicName = "enemy_" + std::to_string(i) + "/odom";
        sub = nh.subscribe<nav_msgs::Odometry>(
                topicName.c_str(), 100,
                boost::bind(GlobalInfo::callbackEnemyPoint, _1, i));
        subs_enemyPoint.push_back(sub);

        topicName = "robot_" + std::to_string(i) + "/ai_status";
        sub = nh.subscribe(
                topicName.c_str(), 100,
                &ObstacleAvoidingPointGenerator::callbackAIStatus,
                &generator[i]);
        subs_AIStatus.push_back(sub);

        topicName = "robot_" + std::to_string(i) + "/avoiding_point";
        ros::Publisher pub = nh.advertise<geometry_msgs::Point>(topicName, 100);
        pubs_avoidingPoint.push_back(pub);
    }

    while (ros::ok()){
        // 存在している味方ロボットのavoidingPointを計算する
        for(int id : GlobalInfo::friendIDs){
            generator[id].setRobotPoint(GlobalInfo::friendPoints[id]);
            generator[id].setTargetPoint(GlobalInfo::targetPoints[id]);
            generator[id].updateAvoidingPoint();

            pubs_avoidingPoint[id].publish(generator[id].getAvoidingPoint());
        }

        ros::spinOnce();
        r.sleep();
    }

    return 0;
}
<|endoftext|>


<commit_before>/* zlib.cc
   Jeremy Barnes, 19 September 2012
   Copyright (c) 2016 Mldb.ai Inc.  All rights reserved.
   This file is part of MLDB. Copyright 2016 mldb.ai Inc. All rights reserved.

   Implementation of the zlib compression format.
*/

#include "compressor.h"
#include "mldb/arch/endian.h"
#include <bzlib.h>
#include "mldb/base/exc_assert.h"
#include <iostream>

using namespace std;

namespace MLDB {

/*****************************************************************************/
/* BZLIB STREAM COMMON                                                        */
/*****************************************************************************/

struct BzlibStreamCommon: public bz_stream {

    typedef Compressor::OnData OnData;
    typedef Compressor::FlushLevel FlushLevel;

    static std::string bzerror(int code)
    {

        switch (code) {
        case BZ_OK: return "OK";
        case BZ_RUN_OK: return "RUN_OK";
        case BZ_FLUSH_OK: return "FLUSH_OK";
        case BZ_FINISH_OK: return "FINISH_OK";
        case BZ_STREAM_END: return "STREAM_END";
        case BZ_SEQUENCE_ERROR: return "SEQUENCE_ERROR";
        case BZ_PARAM_ERROR: return "PARAM_ERROR";
        case BZ_MEM_ERROR: return "MEM_ERROR";
        case BZ_DATA_ERROR: return "DATA_ERROR";
        case BZ_DATA_ERROR_MAGIC: return "DATA_ERROR_MAGIC";
        case BZ_IO_ERROR: return "IO_ERROR";
        case BZ_UNEXPECTED_EOF: return "UNEXPECTED_EOF";
        case BZ_OUTBUFF_FULL: return "OUTBUFF_FULL";
        case BZ_CONFIG_ERROR: return "CONFIG_ERROR";
        default: return "UNKNOWN ERROR";
        }
    }

    BzlibStreamCommon()
    {
        bzalloc = nullptr;
        bzfree = nullptr;
        opaque = nullptr;
    }

    int (*process) (bz_stream * stream, int flush) = nullptr;

    size_t pump(const char * data, size_t len, const OnData & onData,
                int flushLevel)
    {
        //cerr << "pump " << len << " bytes with flush level "
        //     << flushLevel << endl;

        size_t bufSize = 131072;
        char output[bufSize];
        next_in = (char *)data;
        avail_in = len;
        size_t result = 0;

        do {
            next_out = (char *)output;
            avail_out = bufSize;

            int res = process(this, flushLevel);

            //cerr << "pumping " << len << " bytes through with flushLevel "
            //     << flushLevel << " returned " << res << endl;

            size_t bytesWritten = (const char *)next_out - output;

            switch (res) {
            case BZ_RUN_OK:
            case BZ_FLUSH_OK:
                if (bytesWritten)
                    onData(output, bytesWritten);
                result += bytesWritten;
                break;

            case BZ_FINISH_OK:
            case BZ_STREAM_END:
                if (bytesWritten)
                    onData(output, bytesWritten);
                result += bytesWritten;
                return result;

            default:
                throw Exception("bzip2 error: " + bzerror(res));
            };
        } while (avail_in != 0);

        if (flushLevel == BZ_FINISH)
            throw Exception("finished without getting to BZ_STREAM_END");

        return result;
    }

    size_t flush(FlushLevel flushLevel, const OnData & onData)
    {
        int bzlibFlushLevel;
        switch (flushLevel) {
        case Compressor::FLUSH_NONE:       bzlibFlushLevel = BZ_RUN;       break;
        case Compressor::FLUSH_AVAILABLE:  bzlibFlushLevel = BZ_RUN;       break;
        case Compressor::FLUSH_SYNC:       bzlibFlushLevel = BZ_FLUSH;     break;
        case Compressor::FLUSH_RESTART:    bzlibFlushLevel = BZ_FINISH;    break;
        default:
            throw Exception("bad flush level");
        }

        return pump(0, 0, onData, bzlibFlushLevel);
    }

    size_t finish(const OnData & onData)
    {
        return pump(0, 0, onData, BZ_FINISH);
    }
};

/*****************************************************************************/
/* BZIP COMPRESSOR                                                           */
/*****************************************************************************/

struct BzipCompressor : public Compressor, public BzlibStreamCommon {

    typedef Compressor::OnData OnData;
    typedef Compressor::FlushLevel FlushLevel;

    BzipCompressor(int compressionLevel)
    {
        if (compressionLevel == -1)
            compressionLevel = 9; // speed is unaffected by block size, from doc
        int res = BZ2_bzCompressInit(this, compressionLevel, 0 /* verbosity */,
                                     0 /* default work factor */);
        if (res != BZ_OK)
            throw Exception("BZ2_bzCompressInit failed: " + bzerror(res));

        this->process = &BZ2_bzCompress;
    }

    virtual ~BzipCompressor()
    {
        BZ2_bzCompressEnd(this);
    }

    virtual void compress(const char * data, size_t len,
                          const OnData & onData)
    {
        pump(data, len, onData, BZ_RUN);
    }

    virtual void flush(FlushLevel flushLevel, const OnData & onData) override
    {
        BzlibStreamCommon::flush(flushLevel, onData);
    }

    virtual void finish(const OnData & onData) override
    {
        BzlibStreamCommon::finish(onData);
    }
};

static Compressor::Register<BzipCompressor>
registerBzipCompressor("bzip2", {"bz2", "bzip2"});

/*****************************************************************************/
/* BZIP DECOMPRESSOR                                                         */
/*****************************************************************************/

struct BzipDecompressor: public Decompressor, public BzlibStreamCommon {

    typedef Decompressor::OnData OnData;

    static int bz_decompress(bz_stream * stream, int flush)
    {
        ExcAssertEqual(flush, BZ_RUN);
        int result = BZ2_bzDecompress(stream);
        //cerr << "bz2 decompress returned " << bzerror(result) << endl;
        return result;
    }

    BzipDecompressor()
    {
        //cerr << "creating decompressor" << endl;

        int res = BZ2_bzDecompressInit(this, 0 /* verbosity */, 0 /* small */);
        if (res != BZ_OK)
            throw Exception("BZ2_decompressInit failed: " + bzerror(res));
        this->process = &bz_decompress;
    }

    ~BzipDecompressor()
    {
        BZ2_bzDecompressEnd(this);
    }

    virtual int64_t decompressedSize(const char * block, size_t blockLen,
                                     int64_t totalLen) const override
    {
        // TODO: can probably do better than this...
        return LENGTH_UNKNOWN;
    }

    virtual void decompress(const char * data, size_t len,
                            const OnData & onData) override
    {
        pump(data, len, onData, BZ_RUN);
    }

    virtual void finish(const OnData & onData) override
    {
    }
};

static Decompressor::Register<BzipDecompressor>
registerBzipDecompressor("bzip2", {"bz2", "bzip2"});

} // namespace MLDB
<commit_msg>Fixed bzip2 write (TRIVIAL)<commit_after>/* zlib.cc
   Jeremy Barnes, 19 September 2012
   Copyright (c) 2016 Mldb.ai Inc.  All rights reserved.
   This file is part of MLDB. Copyright 2016 mldb.ai Inc. All rights reserved.

   Implementation of the zlib compression format.
*/

#include "compressor.h"
#include "mldb/arch/endian.h"
#include <bzlib.h>
#include "mldb/base/exc_assert.h"
#include <iostream>

using namespace std;

namespace MLDB {

/*****************************************************************************/
/* BZLIB STREAM COMMON                                                        */
/*****************************************************************************/

struct BzlibStreamCommon: public bz_stream {

    typedef Compressor::OnData OnData;
    typedef Compressor::FlushLevel FlushLevel;

    static std::string bzerror(int code)
    {

        switch (code) {
        case BZ_OK: return "OK";
        case BZ_RUN_OK: return "RUN_OK";
        case BZ_FLUSH_OK: return "FLUSH_OK";
        case BZ_FINISH_OK: return "FINISH_OK";
        case BZ_STREAM_END: return "STREAM_END";
        case BZ_SEQUENCE_ERROR: return "SEQUENCE_ERROR";
        case BZ_PARAM_ERROR: return "PARAM_ERROR";
        case BZ_MEM_ERROR: return "MEM_ERROR";
        case BZ_DATA_ERROR: return "DATA_ERROR";
        case BZ_DATA_ERROR_MAGIC: return "DATA_ERROR_MAGIC";
        case BZ_IO_ERROR: return "IO_ERROR";
        case BZ_UNEXPECTED_EOF: return "UNEXPECTED_EOF";
        case BZ_OUTBUFF_FULL: return "OUTBUFF_FULL";
        case BZ_CONFIG_ERROR: return "CONFIG_ERROR";
        default: return "UNKNOWN ERROR";
        }
    }

    BzlibStreamCommon()
    {
        bzalloc = nullptr;
        bzfree = nullptr;
        opaque = nullptr;
    }

    int (*process) (bz_stream * stream, int flush) = nullptr;

    size_t pump(const char * data, size_t len, const OnData & onData,
                int flushLevel)
    {
        //cerr << "pump " << len << " bytes with flush level "
        //     << flushLevel << endl;

        size_t bufSize = 131072;
        char output[bufSize];
        next_in = (char *)data;
        avail_in = len;
        size_t result = 0;

        do {
            next_out = (char *)output;
            avail_out = bufSize;

            int res = process(this, flushLevel);

            //cerr << "pumping " << len << " bytes through with flushLevel "
            //     << flushLevel << " returned " << res << endl;

            size_t bytesWritten = (const char *)next_out - output;

            switch (res) {
            case BZ_OK:
            case BZ_RUN_OK:
            case BZ_FLUSH_OK:
                if (bytesWritten)
                    onData(output, bytesWritten);
                result += bytesWritten;
                break;

            case BZ_FINISH_OK:
            case BZ_STREAM_END:
                if (bytesWritten)
                    onData(output, bytesWritten);
                result += bytesWritten;
                return result;

            default:
                throw Exception("bzip2 error: " + bzerror(res));
            };
        } while (avail_in != 0);

        if (flushLevel == BZ_FINISH)
            throw Exception("finished without getting to BZ_STREAM_END");

        return result;
    }

    size_t flush(FlushLevel flushLevel, const OnData & onData)
    {
        int bzlibFlushLevel;
        switch (flushLevel) {
        case Compressor::FLUSH_NONE:       bzlibFlushLevel = BZ_RUN;       break;
        case Compressor::FLUSH_AVAILABLE:  bzlibFlushLevel = BZ_RUN;       break;
        case Compressor::FLUSH_SYNC:       bzlibFlushLevel = BZ_FLUSH;     break;
        case Compressor::FLUSH_RESTART:    bzlibFlushLevel = BZ_FINISH;    break;
        default:
            throw Exception("bad flush level");
        }

        return pump(0, 0, onData, bzlibFlushLevel);
    }

    size_t finish(const OnData & onData)
    {
        return pump(0, 0, onData, BZ_FINISH);
    }
};

/*****************************************************************************/
/* BZIP COMPRESSOR                                                           */
/*****************************************************************************/

struct BzipCompressor : public Compressor, public BzlibStreamCommon {

    typedef Compressor::OnData OnData;
    typedef Compressor::FlushLevel FlushLevel;

    BzipCompressor(int compressionLevel)
    {
        if (compressionLevel == -1)
            compressionLevel = 9; // speed is unaffected by block size, from doc
        int res = BZ2_bzCompressInit(this, compressionLevel, 0 /* verbosity */,
                                     0 /* default work factor */);
        if (res != BZ_OK)
            throw Exception("BZ2_bzCompressInit failed: " + bzerror(res));

        this->process = &BZ2_bzCompress;
    }

    virtual ~BzipCompressor()
    {
        BZ2_bzCompressEnd(this);
    }

    virtual void compress(const char * data, size_t len,
                          const OnData & onData)
    {
        pump(data, len, onData, BZ_RUN);
    }

    virtual void flush(FlushLevel flushLevel, const OnData & onData) override
    {
        BzlibStreamCommon::flush(flushLevel, onData);
    }

    virtual void finish(const OnData & onData) override
    {
        BzlibStreamCommon::finish(onData);
    }
};

static Compressor::Register<BzipCompressor>
registerBzipCompressor("bzip2", {"bz2", "bzip2"});

/*****************************************************************************/
/* BZIP DECOMPRESSOR                                                         */
/*****************************************************************************/

struct BzipDecompressor: public Decompressor, public BzlibStreamCommon {

    typedef Decompressor::OnData OnData;

    static int bz_decompress(bz_stream * stream, int flush)
    {
        ExcAssertEqual(flush, BZ_RUN);
        int result = BZ2_bzDecompress(stream);
        //cerr << "bz2 decompress returned " << bzerror(result) << endl;
        return result;
    }

    BzipDecompressor()
    {
        //cerr << "creating decompressor" << endl;

        int res = BZ2_bzDecompressInit(this, 0 /* verbosity */, 0 /* small */);
        if (res != BZ_OK)
            throw Exception("BZ2_decompressInit failed: " + bzerror(res));
        this->process = &bz_decompress;
    }

    ~BzipDecompressor()
    {
        BZ2_bzDecompressEnd(this);
    }

    virtual int64_t decompressedSize(const char * block, size_t blockLen,
                                     int64_t totalLen) const override
    {
        // TODO: can probably do better than this...
        return LENGTH_UNKNOWN;
    }

    virtual void decompress(const char * data, size_t len,
                            const OnData & onData) override
    {
        pump(data, len, onData, BZ_RUN);
    }

    virtual void finish(const OnData & onData) override
    {
    }
};

static Decompressor::Register<BzipDecompressor>
registerBzipDecompressor("bzip2", {"bz2", "bzip2"});

} // namespace MLDB
<|endoftext|>


<commit_before>/*
 *  yosys -- Yosys Open SYnthesis Suite
 *
 *  Copyright (C) 2012  Clifford Wolf <clifford@clifford.at>
 *
 *  Permission to use, copy, modify, and/or distribute this software for any
 *  purpose with or without fee is hereby granted, provided that the above
 *  copyright notice and this permission notice appear in all copies.
 *
 *  THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 *  WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 *  MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 *  ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 *  WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 *  ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 *  OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 *
 */

#include "kernel/yosys.h"
#include "kernel/sigtools.h"

USING_YOSYS_NAMESPACE
PRIVATE_NAMESPACE_BEGIN

struct ZinitPass : public Pass {
	ZinitPass() : Pass("zinit", "add inverters so all FF are zero-initialized") { }
	void help() YS_OVERRIDE
	{
		//   |---v---|---v---|---v---|---v---|---v---|---v---|---v---|---v---|---v---|---v---|
		log("\n");
		log("    zinit [options] [selection]\n");
		log("\n");
		log("Add inverters as needed to make all FFs zero-initialized.\n");
		log("\n");
		log("    -all\n");
		log("        also add zero initialization to uninitialized FFs\n");
		log("\n");
	}
	void execute(std::vector<std::string> args, RTLIL::Design *design) YS_OVERRIDE
	{
		bool all_mode = false;

		log_header(design, "Executing ZINIT pass (make all FFs zero-initialized).\n");

		size_t argidx;
		for (argidx = 1; argidx < args.size(); argidx++)
		{
			if (args[argidx] == "-all") {
				all_mode = true;
				continue;
			}
			break;
		}
		extra_args(args, argidx, design);

		for (auto module : design->selected_modules())
		{
			SigMap sigmap(module);
			dict<SigBit, State> initbits;
			pool<SigBit> donebits;

			for (auto wire : module->selected_wires())
			{
				if (wire->attributes.count(ID::init) == 0)
					continue;

				SigSpec wirebits = sigmap(wire);
				Const initval = wire->attributes.at(ID::init);
				wire->attributes.erase(ID::init);

				for (int i = 0; i < GetSize(wirebits) && i < GetSize(initval); i++)
				{
					SigBit bit = wirebits[i];
					State val = initval[i];

					if (val != State::S0 && val != State::S1 && bit.wire != nullptr)
						continue;

					if (initbits.count(bit)) {
						if (initbits.at(bit) != val)
							log_error("Conflicting init values for signal %s (%s = %s != %s).\n",
									log_signal(bit), log_signal(SigBit(wire, i)),
									log_signal(val), log_signal(initbits.at(bit)));
						continue;
					}

					initbits[bit] = val;
				}
			}

			pool<IdString> dff_types = {
				ID($ff), ID($dff), ID($dffe), ID($dffsr), ID($adff),
				ID($_FF_), ID($_DFFE_NN_), ID($_DFFE_NP_), ID($_DFFE_PN_), ID($_DFFE_PP_),
				ID($_DFFSR_NNN_), ID($_DFFSR_NNP_), ID($_DFFSR_NPN_), ID($_DFFSR_NPP_),
				ID($_DFFSR_PNN_), ID($_DFFSR_PNP_), ID($_DFFSR_PPN_), ID($_DFFSR_PPP_),
				ID($_DFF_N_), ID($_DFF_NN0_), ID($_DFF_NN1_), ID($_DFF_NP0_), ID($_DFF_NP1_),
				ID($_DFF_P_), ID($_DFF_PN0_), ID($_DFF_PN1_), ID($_DFF_PP0_), ID($_DFF_PP1_)
			};

			for (auto cell : module->selected_cells())
			{
				if (!dff_types.count(cell->type))
					continue;

				SigSpec sig_d = sigmap(cell->getPort(ID::D));
				SigSpec sig_q = sigmap(cell->getPort(ID::Q));

				if (GetSize(sig_d) < 1 || GetSize(sig_q) < 1)
					continue;

				Const initval;

				for (int i = 0; i < GetSize(sig_q); i++) {
					if (initbits.count(sig_q[i])) {
						initval.bits.push_back(initbits.at(sig_q[i]));
						donebits.insert(sig_q[i]);
					} else
						initval.bits.push_back(all_mode ? State::S0 : State::Sx);
				}

				Wire *initwire = module->addWire(NEW_ID, GetSize(initval));
				initwire->attributes[ID::init] = initval;

				for (int i = 0; i < GetSize(initwire); i++)
					if (initval.bits.at(i) == State::S1)
					{
						sig_d[i] = module->NotGate(NEW_ID, sig_d[i]);
						module->addNotGate(NEW_ID, SigSpec(initwire, i), sig_q[i]);
						initwire->attributes[ID::init].bits.at(i) = State::S0;
					}
					else
					{
						module->connect(sig_q[i], SigSpec(initwire, i));
					}

				log("FF init value for cell %s (%s): %s = %s\n", log_id(cell), log_id(cell->type),
						log_signal(sig_q), log_signal(initval));

				cell->setPort(ID::D, sig_d);
				cell->setPort(ID::Q, initwire);

				/*if (cell->type.in(ID($_DFFSR_NNN_), ID($_DFFSR_NNP_), ID($_DFFSR_NPN_), ID($_DFFSR_NPP_),
							ID($_DFFSR_PNN_), ID($_DFFSR_PNP_), ID($_DFFSR_PPN_), ID($_DFFSR_PPP_)))
				{
					// TODO: I think I need a $_DFFRS_* cell where S has priority over R...
					std::swap(cell->connections_.at(ID(R)), cell->connections_.at(ID(S)));
				}
				else*/ if (cell->type.in(ID($_DFF_NN0_), ID($_DFF_NN1_), ID($_DFF_NP0_), ID($_DFF_NP1_),
							ID($_DFF_PN0_), ID($_DFF_PN1_), ID($_DFF_PP0_), ID($_DFF_PP1_)))
				{
					std::string t = cell->type.str();
					t[8] = (t[8] == '0' ? '1' : '0');
					cell->type = t;
				}
			}

			if (!design->selected_whole_module(module))
				for (auto &it : initbits)
					if (donebits.count(it.first) == 0)
						log_error("Failed to handle init bit %s = %s.\n", log_signal(it.first), log_signal(it.second));
		}
	}
} ZinitPass;

PRIVATE_NAMESPACE_END
<commit_msg>zinit: operate on $adff, erase (* init *) entries on consumption<commit_after>/*
 *  yosys -- Yosys Open SYnthesis Suite
 *
 *  Copyright (C) 2012  Clifford Wolf <clifford@clifford.at>
 *
 *  Permission to use, copy, modify, and/or distribute this software for any
 *  purpose with or without fee is hereby granted, provided that the above
 *  copyright notice and this permission notice appear in all copies.
 *
 *  THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 *  WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 *  MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 *  ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 *  WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 *  ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 *  OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 *
 */

#include "kernel/yosys.h"
#include "kernel/sigtools.h"

USING_YOSYS_NAMESPACE
PRIVATE_NAMESPACE_BEGIN

struct ZinitPass : public Pass {
	ZinitPass() : Pass("zinit", "add inverters so all FF are zero-initialized") { }
	void help() YS_OVERRIDE
	{
		//   |---v---|---v---|---v---|---v---|---v---|---v---|---v---|---v---|---v---|---v---|
		log("\n");
		log("    zinit [options] [selection]\n");
		log("\n");
		log("Add inverters as needed to make all FFs zero-initialized.\n");
		log("\n");
		log("    -all\n");
		log("        also add zero initialization to uninitialized FFs\n");
		log("\n");
	}
	void execute(std::vector<std::string> args, RTLIL::Design *design) YS_OVERRIDE
	{
		bool all_mode = false;

		log_header(design, "Executing ZINIT pass (make all FFs zero-initialized).\n");

		size_t argidx;
		for (argidx = 1; argidx < args.size(); argidx++)
		{
			if (args[argidx] == "-all") {
				all_mode = true;
				continue;
			}
			break;
		}
		extra_args(args, argidx, design);

		for (auto module : design->selected_modules())
		{
			SigMap sigmap(module);
			dict<SigBit, std::pair<State,SigBit>> initbits;

			for (auto wire : module->selected_wires())
			{
				if (wire->attributes.count(ID::init) == 0)
					continue;

				SigSpec wirebits = sigmap(wire);
				Const initval = wire->attributes.at(ID::init);

				for (int i = 0; i < GetSize(wirebits) && i < GetSize(initval); i++)
				{
					SigBit bit = wirebits[i];
					State val = initval[i];

					if (val != State::S0 && val != State::S1 && bit.wire != nullptr)
						continue;

					if (initbits.count(bit)) {
						if (initbits.at(bit).first != val)
							log_error("Conflicting init values for signal %s (%s = %s != %s).\n",
									log_signal(bit), log_signal(SigBit(wire, i)),
									log_signal(val), log_signal(initbits.at(bit).first));
						continue;
					}

					initbits[bit] = std::make_pair(val,SigBit(wire,i));
				}
			}

			pool<IdString> dff_types = {
								// FIXME: It would appear that supporting
								//    $dffsr/$_DFFSR_* would require a new
								//    cell type where S has priority over R
				ID($ff), ID($dff), ID($dffe), /*ID($dffsr),*/ ID($adff),
				ID($_FF_), ID($_DFFE_NN_), ID($_DFFE_NP_), ID($_DFFE_PN_), ID($_DFFE_PP_),
				/*ID($_DFFSR_NNN_), ID($_DFFSR_NNP_), ID($_DFFSR_NPN_), ID($_DFFSR_NPP_),
				ID($_DFFSR_PNN_), ID($_DFFSR_PNP_), ID($_DFFSR_PPN_), ID($_DFFSR_PPP_),*/
				ID($_DFF_N_), ID($_DFF_NN0_), ID($_DFF_NN1_), ID($_DFF_NP0_), ID($_DFF_NP1_),
				ID($_DFF_P_), ID($_DFF_PN0_), ID($_DFF_PN1_), ID($_DFF_PP0_), ID($_DFF_PP1_)
			};

			for (auto cell : module->selected_cells())
			{
				if (!dff_types.count(cell->type))
					continue;

				SigSpec sig_d = sigmap(cell->getPort(ID::D));
				SigSpec sig_q = sigmap(cell->getPort(ID::Q));

				if (GetSize(sig_d) < 1 || GetSize(sig_q) < 1)
					continue;

				Const initval;

				for (int i = 0; i < GetSize(sig_q); i++) {
					if (initbits.count(sig_q[i])) {
						const auto &d = initbits.at(sig_q[i]);
						initval.bits.push_back(d.first);
						const auto &b = d.second;
						b.wire->attributes.at(ID::init)[b.offset];
					} else
						initval.bits.push_back(all_mode ? State::S0 : State::Sx);
				}

				Wire *initwire = module->addWire(NEW_ID, GetSize(initval));
				initwire->attributes[ID::init] = initval;

				for (int i = 0; i < GetSize(initwire); i++)
					if (initval.bits.at(i) == State::S1)
					{
						sig_d[i] = module->NotGate(NEW_ID, sig_d[i]);
						module->addNotGate(NEW_ID, SigSpec(initwire, i), sig_q[i]);
						initwire->attributes[ID::init].bits.at(i) = State::S0;
					}
					else
					{
						module->connect(sig_q[i], SigSpec(initwire, i));
					}

				log("FF init value for cell %s (%s): %s = %s\n", log_id(cell), log_id(cell->type),
						log_signal(sig_q), log_signal(initval));

				cell->setPort(ID::D, sig_d);
				cell->setPort(ID::Q, initwire);

				if (cell->type == ID($adff)) {
					auto val = cell->getParam(ID::ARST_VALUE);
					for (auto &b : val)
						b = (b == State::S1 ? State::S0 : State::S1);
					cell->setParam(ID::ARST_VALUE, std::move(val));
				}
				else if (cell->type.in(ID($_DFF_NN0_), ID($_DFF_NN1_), ID($_DFF_NP0_), ID($_DFF_NP1_),
							ID($_DFF_PN0_), ID($_DFF_PN1_), ID($_DFF_PP0_), ID($_DFF_PP1_)))
				{
					std::string t = cell->type.str();
					t[8] = (t[8] == '0' ? '1' : '0');
					cell->type = t;
				}
			}
		}
	}
} ZinitPass;

PRIVATE_NAMESPACE_END
<|endoftext|>


<commit_before>#include <Python.h>
#include <structmember.h>

#include <poppler/cpp/poppler-document.h>
#include <poppler/cpp/poppler-global.h>
#include <poppler/cpp/poppler-page.h>

#include <string>
#include <vector>

static PyObject* PdftotextError;

typedef struct {
    PyObject_HEAD
    int page_count;
    PyObject* data;
    poppler::document* doc;
} PDF;

static void PDF_clear(PDF* self) {
    self->page_count = 0;
    delete self->doc;
    self->doc = NULL;
    Py_CLEAR(self->data);
}

static int PDF_load_data(PDF* self, PyObject* args, PyObject* kwds) {
    PyObject* arg;
    static char* kwlist[] = {(char*)"pdf_file", NULL};

    if (!PyArg_ParseTupleAndKeywords(args, kwds, "O", kwlist, &arg)) {
        return -1;
    }
    #if PY_MAJOR_VERSION >= 3
    self->data = PyObject_CallMethod(arg, "read", NULL);
    #else
    self->data = PyObject_CallMethod(arg, (char*)"read", NULL);
    #endif
    if (self->data == NULL) {
        return -1;
    }
    return 0;
}

static int PDF_create_doc(PDF* self) {
    Py_ssize_t len;
    char* buf;

    if (PyBytes_AsStringAndSize(self->data, &buf, &len) < 0) {
        Py_CLEAR(self->data);
        return -1;
    }
    self->doc = poppler::document::load_from_raw_data(buf, len);
    if (self->doc == NULL) {
        PyErr_Format(PdftotextError, "Poppler error creating document");
        Py_CLEAR(self->data);
        return -1;
    }
    return 0;
}

static int PDF_init(PDF* self, PyObject* args, PyObject* kwds) {
    PDF_clear(self);
    if (PDF_load_data(self, args, kwds) < 0) {
        return -1;
    }
    if (PDF_create_doc(self) < 0) {
        return -1;
    }
    self->page_count = self->doc->pages();
    return 0;
}

static void PDF_dealloc(PDF* self) {
    PDF_clear(self);
    Py_TYPE(self)->tp_free((PyObject*)self);
}

static PyObject* PDF_read_page(PDF* self, int page_number) {
    const poppler::page* page;
    std::vector<char> page_utf8;

    page = self->doc->create_page(page_number);
    if (page == NULL) {
        return PyErr_Format(PdftotextError, "Poppler error creating page");
    }
    page_utf8 = page->text().to_utf8();
    delete page;
    return PyUnicode_DecodeUTF8(page_utf8.data(), page_utf8.size(), NULL);
}

static Py_ssize_t PDF_len(PyObject* obj) {
    PDF* self = (PDF*)obj;
    return self->page_count;
}

static PyObject* PDF_getitem(PyObject* obj, Py_ssize_t i) {
    PDF* self = (PDF*)obj;

    if (i < 0 || i >= self->page_count) {
        return PyErr_Format(PyExc_IndexError, "Index out of range");
    }
    return PDF_read_page(self, i);
}

static PySequenceMethods PDF_sequence_methods = {
    PDF_len,      // sq_length (__len__)
    0,            // sq_concat
    0,            // sq_repeat
    PDF_getitem,  // sq_item (__getitem__)
};

static PyTypeObject PDFType = {
    PyVarObject_HEAD_INIT(NULL, 0)
    "pdftotext.PDF",                           // tp_name
    sizeof(PDF),                               // tp_basicsize
    0,                                         // tp_itemsize
    (destructor)PDF_dealloc,                   // tp_dealloc
    0,                                         // tp_print
    0,                                         // tp_getattr
    0,                                         // tp_setattr
    0,                                         // tp_reserved
    0,                                         // tp_repr
    0,                                         // tp_as_number
    &PDF_sequence_methods,                     // tp_as_sequence
    0,                                         // tp_as_mapping
    0,                                         // tp_hash
    0,                                         // tp_call
    0,                                         // tp_str
    0,                                         // tp_getattro
    0,                                         // tp_setattro
    0,                                         // tp_as_buffer
    Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE,  // tp_flags
    "PDF document.",                           // tp_doc
    0,                                         // tp_traverse
    0,                                         // tp_clear
    0,                                         // tp_richcompare
    0,                                         // tp_weaklistoffset
    0,                                         // tp_iter
    0,                                         // tp_iternext
    0,                                         // tp_methods
    0,                                         // tp_members
    0,                                         // tp_getset
    0,                                         // tp_base
    0,                                         // tp_dict
    0,                                         // tp_descr_get
    0,                                         // tp_descr_set
    0,                                         // tp_dictoffset
    (initproc)PDF_init,                        // tp_init
};

#if POPPLER_CPP_AT_LEAST_0_30_0
static void do_nothing(const std::string&, void*) {}
#endif

#if PY_MAJOR_VERSION >= 3
static PyModuleDef pdftotextmodule = {
    PyModuleDef_HEAD_INIT,
    "pdftotext",
    "Simple PDF text extraction.",
};

PyMODINIT_FUNC PyInit_pdftotext() {
    PyObject* module;

    PDFType.tp_new = PyType_GenericNew;
    if (PyType_Ready(&PDFType) < 0) {
        return NULL;
    }

    module = PyModule_Create(&pdftotextmodule);
    if (module == NULL) {
        return NULL;
    }

    Py_INCREF(&PDFType);
    PyModule_AddObject(module, "PDF", (PyObject*)&PDFType);

    PdftotextError = PyErr_NewExceptionWithDoc(
        "pdftotext.Error", "PDF error.", NULL, NULL);
    Py_INCREF(PdftotextError);
    PyModule_AddObject(module, "Error", PdftotextError);

    #if POPPLER_CPP_AT_LEAST_0_30_0
    poppler::set_debug_error_function(do_nothing, NULL);
    #endif

    return module;
}
#else
static PyMethodDef module_methods[] = {
    {NULL},  // Sentinel
};

PyMODINIT_FUNC initpdftotext() {
    PyObject* module;

    PDFType.tp_new = PyType_GenericNew;
    if (PyType_Ready(&PDFType) < 0) {
        return;
    }

    module = Py_InitModule3(
        "pdftotext", module_methods, "Simple PDF text extraction.");
    if (module == NULL) {
        return;
    }

    Py_INCREF(&PDFType);
    PyModule_AddObject(module, "PDF", (PyObject*)&PDFType);

    PdftotextError = PyErr_NewExceptionWithDoc(
        (char*)"pdftotext.Error", (char*)"PDF error.", NULL, NULL);
    Py_INCREF(PdftotextError);
    PyModule_AddObject(module, "Error", PdftotextError);

    #if POPPLER_CPP_AT_LEAST_0_30_0
    poppler::set_debug_error_function(do_nothing, NULL);
    #endif
}
#endif
<commit_msg>Deduplicate<commit_after>#include <Python.h>
#include <structmember.h>

#include <poppler/cpp/poppler-document.h>
#include <poppler/cpp/poppler-global.h>
#include <poppler/cpp/poppler-page.h>

#include <string>
#include <vector>

static PyObject* PdftotextError;

typedef struct {
    PyObject_HEAD
    int page_count;
    PyObject* data;
    poppler::document* doc;
} PDF;

static void PDF_clear(PDF* self) {
    self->page_count = 0;
    delete self->doc;
    self->doc = NULL;
    Py_CLEAR(self->data);
}

static int PDF_load_data(PDF* self, PyObject* args, PyObject* kwds) {
    PyObject* arg;
    static char* kwlist[] = {(char*)"pdf_file", NULL};

    if (!PyArg_ParseTupleAndKeywords(args, kwds, "O", kwlist, &arg)) {
        return -1;
    }
    #if PY_MAJOR_VERSION >= 3
    self->data = PyObject_CallMethod(arg, "read", NULL);
    #else
    self->data = PyObject_CallMethod(arg, (char*)"read", NULL);
    #endif
    if (self->data == NULL) {
        return -1;
    }
    return 0;
}

static int PDF_create_doc(PDF* self) {
    Py_ssize_t len;
    char* buf;

    if (PyBytes_AsStringAndSize(self->data, &buf, &len) < 0) {
        return -1;
    }
    self->doc = poppler::document::load_from_raw_data(buf, len);
    if (self->doc == NULL) {
        PyErr_Format(PdftotextError, "Poppler error creating document");
        return -1;
    }
    return 0;
}

static int PDF_init(PDF* self, PyObject* args, PyObject* kwds) {
    PDF_clear(self);
    if (PDF_load_data(self, args, kwds) < 0) {
        return -1;
    }
    if (PDF_create_doc(self) < 0) {
        Py_CLEAR(self->data);
        return -1;
    }
    self->page_count = self->doc->pages();
    return 0;
}

static void PDF_dealloc(PDF* self) {
    PDF_clear(self);
    Py_TYPE(self)->tp_free((PyObject*)self);
}

static PyObject* PDF_read_page(PDF* self, int page_number) {
    const poppler::page* page;
    std::vector<char> page_utf8;

    page = self->doc->create_page(page_number);
    if (page == NULL) {
        return PyErr_Format(PdftotextError, "Poppler error creating page");
    }
    page_utf8 = page->text().to_utf8();
    delete page;
    return PyUnicode_DecodeUTF8(page_utf8.data(), page_utf8.size(), NULL);
}

static Py_ssize_t PDF_len(PyObject* obj) {
    PDF* self = (PDF*)obj;
    return self->page_count;
}

static PyObject* PDF_getitem(PyObject* obj, Py_ssize_t i) {
    PDF* self = (PDF*)obj;

    if (i < 0 || i >= self->page_count) {
        return PyErr_Format(PyExc_IndexError, "Index out of range");
    }
    return PDF_read_page(self, i);
}

static PySequenceMethods PDF_sequence_methods = {
    PDF_len,      // sq_length (__len__)
    0,            // sq_concat
    0,            // sq_repeat
    PDF_getitem,  // sq_item (__getitem__)
};

static PyTypeObject PDFType = {
    PyVarObject_HEAD_INIT(NULL, 0)
    "pdftotext.PDF",                           // tp_name
    sizeof(PDF),                               // tp_basicsize
    0,                                         // tp_itemsize
    (destructor)PDF_dealloc,                   // tp_dealloc
    0,                                         // tp_print
    0,                                         // tp_getattr
    0,                                         // tp_setattr
    0,                                         // tp_reserved
    0,                                         // tp_repr
    0,                                         // tp_as_number
    &PDF_sequence_methods,                     // tp_as_sequence
    0,                                         // tp_as_mapping
    0,                                         // tp_hash
    0,                                         // tp_call
    0,                                         // tp_str
    0,                                         // tp_getattro
    0,                                         // tp_setattro
    0,                                         // tp_as_buffer
    Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE,  // tp_flags
    "PDF document.",                           // tp_doc
    0,                                         // tp_traverse
    0,                                         // tp_clear
    0,                                         // tp_richcompare
    0,                                         // tp_weaklistoffset
    0,                                         // tp_iter
    0,                                         // tp_iternext
    0,                                         // tp_methods
    0,                                         // tp_members
    0,                                         // tp_getset
    0,                                         // tp_base
    0,                                         // tp_dict
    0,                                         // tp_descr_get
    0,                                         // tp_descr_set
    0,                                         // tp_dictoffset
    (initproc)PDF_init,                        // tp_init
};

#if POPPLER_CPP_AT_LEAST_0_30_0
static void do_nothing(const std::string&, void*) {}
#endif

#if PY_MAJOR_VERSION >= 3
static PyModuleDef pdftotextmodule = {
    PyModuleDef_HEAD_INIT,
    "pdftotext",
    "Simple PDF text extraction.",
};

PyMODINIT_FUNC PyInit_pdftotext() {
    PyObject* module;

    PDFType.tp_new = PyType_GenericNew;
    if (PyType_Ready(&PDFType) < 0) {
        return NULL;
    }

    module = PyModule_Create(&pdftotextmodule);
    if (module == NULL) {
        return NULL;
    }

    Py_INCREF(&PDFType);
    PyModule_AddObject(module, "PDF", (PyObject*)&PDFType);

    PdftotextError = PyErr_NewExceptionWithDoc(
        "pdftotext.Error", "PDF error.", NULL, NULL);
    Py_INCREF(PdftotextError);
    PyModule_AddObject(module, "Error", PdftotextError);

    #if POPPLER_CPP_AT_LEAST_0_30_0
    poppler::set_debug_error_function(do_nothing, NULL);
    #endif

    return module;
}
#else
static PyMethodDef module_methods[] = {
    {NULL},  // Sentinel
};

PyMODINIT_FUNC initpdftotext() {
    PyObject* module;

    PDFType.tp_new = PyType_GenericNew;
    if (PyType_Ready(&PDFType) < 0) {
        return;
    }

    module = Py_InitModule3(
        "pdftotext", module_methods, "Simple PDF text extraction.");
    if (module == NULL) {
        return;
    }

    Py_INCREF(&PDFType);
    PyModule_AddObject(module, "PDF", (PyObject*)&PDFType);

    PdftotextError = PyErr_NewExceptionWithDoc(
        (char*)"pdftotext.Error", (char*)"PDF error.", NULL, NULL);
    Py_INCREF(PdftotextError);
    PyModule_AddObject(module, "Error", PdftotextError);

    #if POPPLER_CPP_AT_LEAST_0_30_0
    poppler::set_debug_error_function(do_nothing, NULL);
    #endif
}
#endif
<|endoftext|>