<commit_before>#include "vector.h"
#include "MinIMU9.h"
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <sys/time.h>
#include <system_error>

int millis()
{
    struct timeval tv;
    gettimeofday(&tv, NULL);
    return (tv.tv_sec) * 1000 + (tv.tv_usec)/1000;
}

void print(matrix m)
{
    printf("%7.4f %7.4f %7.4f %7.4f %7.4f %7.4f %7.4f %7.4f %7.4f",
           m(0,0), m(0,1), m(0,2),
           m(1,0), m(1,1), m(1,2),
           m(2,0), m(2,1), m(2,2));
}

void streamRawValues(IMU& imu)
{
    imu.enableSensors();
    while(1)
    {
        imu.read();
        printf("m: %7d %7d %7d  a: %7d %7d %7d  g: %7d %7d %7d\n",
               imu.m[0], imu.m[1], imu.m[2],
               imu.a[0], imu.a[1], imu.a[2],
               imu.g[0], imu.g[1], imu.g[2]
        );
        usleep(20*1000);
    }
}

static matrix updateMatrix(const vector& w, float dt)
{
    matrix u = matrix::Identity();
    u(2,0) = -w(1) * dt;
    u(0,2) =  w(1) * dt;

    u(0,1) = -w(2) * dt;
    u(1,0) =  w(2) * dt;

    u(1,2) = -w(0) * dt;
    u(2,1) =  w(0) * dt;
    return u;
}

static matrix normalize(const matrix & m)
{
    //float error = m.row(0).dot(m.row(1));
    matrix norm;
    norm.row(0) = m.row(0) + m.row(1).cross(m.row(2))/10;
    norm.row(1) = m.row(1) + m.row(2).cross(m.row(0))/10;
    norm.row(2) = m.row(2) + m.row(0).cross(m.row(1))/10;
    norm.row(0).normalize();
    norm.row(1).normalize();
    norm.row(2).normalize();
    return norm;
}

matrix rotationFromCompass(const vector& acceleration, const vector& magnetic_field)
{
    vector up = acceleration;     // usually true
    vector magnetic_east = magnetic_field.cross(up);
    vector east = magnetic_east;  // a rough approximation
    vector north = up.cross(east);

    matrix rotationFromCompass;
    rotationFromCompass.row(0) = east;
    rotationFromCompass.row(1) = north;
    rotationFromCompass.row(2) = up;
    rotationFromCompass.row(0).normalize();
    rotationFromCompass.row(1).normalize();
    rotationFromCompass.row(2).normalize();

    return rotationFromCompass;
}

float heading(matrix rotation)
{
    // The board's x axis in earth coordinates.
    vector x = rotation.col(0);
    x.normalize();
    x(2) = 0;
    //float heading_weight = x.norm();

    // 0 = east, pi/2 = north
    return atan2(x(1), x(0));
}

typedef void fuse_function(matrix& rotation, float dt, const vector& angular_velocity,
                  const vector& acceleration, const vector& magnetic_field);

void fuse_compass_only(matrix& rotation, float dt, const vector& angular_velocity,
  const vector& acceleration, const vector& magnetic_field)
{
    rotation = rotationFromCompass(acceleration, magnetic_field);
}

void fuse_gyro_only(matrix& rotation, float dt, const vector& angular_velocity,
  const vector& acceleration, const vector& magnetic_field)
{
   rotation *= updateMatrix(angular_velocity, dt);
   rotation = normalize(rotation);
}

#define Kp_ROLLPITCH 1
#define Ki_ROLLPITCH 0.00002
#define Kp_YAW 1
#define Ki_YAW 0.00002

#define PI 3.14159265

void fuse_default(matrix& rotation, float dt, const vector& angular_velocity,
  const vector& acceleration, const vector& magnetic_field)
{
    matrix rotationCompass = rotationFromCompass(acceleration, magnetic_field);

    // We trust the accelerometer more if it is telling us 1G.
    float accel_weight = 1 - 2*abs(1 - acceleration.norm());
    if (accel_weight < 0){ accel_weight = 0; }

    // Add a "torque" that makes our up vector (rotation.row(2))
    // get closer to the acceleration vector.
    vector errorRollPitch = acceleration.cross(rotation.row(2)) * accel_weight;

    // Add a "torque" that makes our east vector (rotation.row(0))
    // get closer to east vector calculated from the compass.
    vector errorYaw = rotationCompass.row(0).cross(rotation.row(0));

    vector drift_correction = errorYaw * Kp_YAW + errorRollPitch * Kp_ROLLPITCH;

    rotation *= updateMatrix(angular_velocity + drift_correction, dt);
    rotation = normalize(rotation);
}

// DCM algorithm: http://diydrones.com/forum/topics/robust-estimator-of-the

void ahrs(IMU& imu, fuse_function * fuse_func)
{
    imu.loadCalibration();
    imu.enableSensors();
    imu.measureOffsets();

    fprintf(stderr, "Gyro offset: %7f %7f %7f\nAccel offset: %7f %7f %7f (%7f)\n",
            imu.gyro_offset(0), imu.gyro_offset(1), imu.gyro_offset(2),
            imu.accel_offset(0), imu.accel_offset(1), imu.accel_offset(2),
            imu.accel_offset.norm());

    // The rotation matrix that can convert a vector in body coordinates
    // to ground coordinates.
    matrix rotation = matrix::Identity();

    int start = millis(); // truncate 64-bit return value
    while(1)
    {
        int last_start = start;
        start = millis();
        float dt = (start-last_start)/1000.0;
        if (dt < 0){ throw "time went backwards"; }

        vector angular_velocity = imu.readGyro();
        vector acceleration = imu.readAcc();
        vector magnetic_field = imu.readMag();

        fuse_func(rotation, dt, angular_velocity, acceleration, magnetic_field);

        //fprintf(stderr, "g: %8d %8d %8d\n", gyro.g(0), gyro.g(1), gyro.g(2));
        //fprintf(stderr, "m: %7.4f %7.4f %7.4f  m_raw: %8d %8d %8d\n", magnetic_field(0), magnetic_field(1), magnetic_field(2), compass.m(0), compass.m(1), compass.m(2));
        //fprintf(stderr, "dt: %7.4f  w: %7.4f %7.4f %7.4f\n", dt, angular_velocity(0), angular_velocity(1), angular_velocity(2));

        //rotation = rotationFromCompass;  // TMPHAX

        printf("%7.4f %7.4f %7.4f %7.4f %7.4f %7.4f %7.4f %7.4f %7.4f  %7.4f %7.4f %7.4f  %7.4f %7.4f %7.4f\n",
               rotation(0,0), rotation(0,1), rotation(0,2),
               rotation(1,0), rotation(1,1), rotation(1,2),
               rotation(2,0), rotation(2,1), rotation(2,2),
               acceleration(0), acceleration(1), acceleration(2),
               magnetic_field(0), magnetic_field(1), magnetic_field(2));
        fflush(stdout);

        //std::cout << rotation;

        // Ensure that each iteration of the loop takes at least 20 ms.
        while(millis() - start < 20)
        {
            usleep(1000);
        }
    }
}

int main(int argc, char *argv[])
{
    try
    {
        MinIMU9 imu("/dev/i2c-0");

        imu.checkConnection();

        if (argc > 1)
        {
            const char * action = argv[1];
            if (0 == strcmp("raw", action))
            {
                streamRawValues(imu);
            }
            else if (0 == strcmp("gyro-only", action))
            {
                ahrs(imu, &fuse_gyro_only);
            }
            else if (0 == strcmp("compass-only", action))
            {
                ahrs(imu, &fuse_compass_only);
            }
            else
            {
                fprintf(stderr, "Unknown action '%s'.\n", action);
                return 3;
            }
        }
        else
        {
            ahrs(imu, &fuse_default);
        }
        return 0;
    }
    catch(const std::system_error & error)
    {
        std::string what = error.what();
        const std::error_code & code = error.code();
        std::cerr << "Error: " << what << "  " << code.message() << " (" << code << ")\n";
    }
    catch(const std::exception & error)
    {
        std::cerr << "Error: " << error.what() << '\n';
    }
    return 1;
}
<commit_msg>Fixed the format of the raw output.  It needs to be compatible with spreadsheet programs and the imu-calibrator.py script.<commit_after>#include "vector.h"
#include "MinIMU9.h"
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <sys/time.h>
#include <system_error>

int millis()
{
    struct timeval tv;
    gettimeofday(&tv, NULL);
    return (tv.tv_sec) * 1000 + (tv.tv_usec)/1000;
}

void print(matrix m)
{
    printf("%7.4f %7.4f %7.4f %7.4f %7.4f %7.4f %7.4f %7.4f %7.4f",
           m(0,0), m(0,1), m(0,2),
           m(1,0), m(1,1), m(1,2),
           m(2,0), m(2,1), m(2,2));
}

void streamRawValues(IMU& imu)
{
    imu.enableSensors();
    while(1)
    {
        imu.read();
        printf("%7d %7d %7d  %7d %7d %7d  %7d %7d %7d\n",
               imu.m[0], imu.m[1], imu.m[2],
               imu.a[0], imu.a[1], imu.a[2],
               imu.g[0], imu.g[1], imu.g[2]
        );
        usleep(20*1000);
    }
}

static matrix updateMatrix(const vector& w, float dt)
{
    matrix u = matrix::Identity();
    u(2,0) = -w(1) * dt;
    u(0,2) =  w(1) * dt;

    u(0,1) = -w(2) * dt;
    u(1,0) =  w(2) * dt;

    u(1,2) = -w(0) * dt;
    u(2,1) =  w(0) * dt;
    return u;
}

static matrix normalize(const matrix & m)
{
    //float error = m.row(0).dot(m.row(1));
    matrix norm;
    norm.row(0) = m.row(0) + m.row(1).cross(m.row(2))/10;
    norm.row(1) = m.row(1) + m.row(2).cross(m.row(0))/10;
    norm.row(2) = m.row(2) + m.row(0).cross(m.row(1))/10;
    norm.row(0).normalize();
    norm.row(1).normalize();
    norm.row(2).normalize();
    return norm;
}

matrix rotationFromCompass(const vector& acceleration, const vector& magnetic_field)
{
    vector up = acceleration;     // usually true
    vector magnetic_east = magnetic_field.cross(up);
    vector east = magnetic_east;  // a rough approximation
    vector north = up.cross(east);

    matrix rotationFromCompass;
    rotationFromCompass.row(0) = east;
    rotationFromCompass.row(1) = north;
    rotationFromCompass.row(2) = up;
    rotationFromCompass.row(0).normalize();
    rotationFromCompass.row(1).normalize();
    rotationFromCompass.row(2).normalize();

    return rotationFromCompass;
}

float heading(matrix rotation)
{
    // The board's x axis in earth coordinates.
    vector x = rotation.col(0);
    x.normalize();
    x(2) = 0;
    //float heading_weight = x.norm();

    // 0 = east, pi/2 = north
    return atan2(x(1), x(0));
}

typedef void fuse_function(matrix& rotation, float dt, const vector& angular_velocity,
                  const vector& acceleration, const vector& magnetic_field);

void fuse_compass_only(matrix& rotation, float dt, const vector& angular_velocity,
  const vector& acceleration, const vector& magnetic_field)
{
    rotation = rotationFromCompass(acceleration, magnetic_field);
}

void fuse_gyro_only(matrix& rotation, float dt, const vector& angular_velocity,
  const vector& acceleration, const vector& magnetic_field)
{
   rotation *= updateMatrix(angular_velocity, dt);
   rotation = normalize(rotation);
}

#define Kp_ROLLPITCH 1
#define Ki_ROLLPITCH 0.00002
#define Kp_YAW 1
#define Ki_YAW 0.00002

#define PI 3.14159265

void fuse_default(matrix& rotation, float dt, const vector& angular_velocity,
  const vector& acceleration, const vector& magnetic_field)
{
    matrix rotationCompass = rotationFromCompass(acceleration, magnetic_field);

    // We trust the accelerometer more if it is telling us 1G.
    float accel_weight = 1 - 2*abs(1 - acceleration.norm());
    if (accel_weight < 0){ accel_weight = 0; }

    // Add a "torque" that makes our up vector (rotation.row(2))
    // get closer to the acceleration vector.
    vector errorRollPitch = acceleration.cross(rotation.row(2)) * accel_weight;

    // Add a "torque" that makes our east vector (rotation.row(0))
    // get closer to east vector calculated from the compass.
    vector errorYaw = rotationCompass.row(0).cross(rotation.row(0));

    vector drift_correction = errorYaw * Kp_YAW + errorRollPitch * Kp_ROLLPITCH;

    rotation *= updateMatrix(angular_velocity + drift_correction, dt);
    rotation = normalize(rotation);
}

// DCM algorithm: http://diydrones.com/forum/topics/robust-estimator-of-the

void ahrs(IMU& imu, fuse_function * fuse_func)
{
    imu.loadCalibration();
    imu.enableSensors();
    imu.measureOffsets();

    fprintf(stderr, "Gyro offset: %7f %7f %7f\nAccel offset: %7f %7f %7f (%7f)\n",
            imu.gyro_offset(0), imu.gyro_offset(1), imu.gyro_offset(2),
            imu.accel_offset(0), imu.accel_offset(1), imu.accel_offset(2),
            imu.accel_offset.norm());

    // The rotation matrix that can convert a vector in body coordinates
    // to ground coordinates.
    matrix rotation = matrix::Identity();

    int start = millis(); // truncate 64-bit return value
    while(1)
    {
        int last_start = start;
        start = millis();
        float dt = (start-last_start)/1000.0;
        if (dt < 0){ throw "time went backwards"; }

        vector angular_velocity = imu.readGyro();
        vector acceleration = imu.readAcc();
        vector magnetic_field = imu.readMag();

        fuse_func(rotation, dt, angular_velocity, acceleration, magnetic_field);

        //fprintf(stderr, "g: %8d %8d %8d\n", gyro.g(0), gyro.g(1), gyro.g(2));
        //fprintf(stderr, "m: %7.4f %7.4f %7.4f  m_raw: %8d %8d %8d\n", magnetic_field(0), magnetic_field(1), magnetic_field(2), compass.m(0), compass.m(1), compass.m(2));
        //fprintf(stderr, "dt: %7.4f  w: %7.4f %7.4f %7.4f\n", dt, angular_velocity(0), angular_velocity(1), angular_velocity(2));

        //rotation = rotationFromCompass;  // TMPHAX

        printf("%7.4f %7.4f %7.4f %7.4f %7.4f %7.4f %7.4f %7.4f %7.4f  %7.4f %7.4f %7.4f  %7.4f %7.4f %7.4f\n",
               rotation(0,0), rotation(0,1), rotation(0,2),
               rotation(1,0), rotation(1,1), rotation(1,2),
               rotation(2,0), rotation(2,1), rotation(2,2),
               acceleration(0), acceleration(1), acceleration(2),
               magnetic_field(0), magnetic_field(1), magnetic_field(2));
        fflush(stdout);

        //std::cout << rotation;

        // Ensure that each iteration of the loop takes at least 20 ms.
        while(millis() - start < 20)
        {
            usleep(1000);
        }
    }
}

int main(int argc, char *argv[])
{
    try
    {
        MinIMU9 imu("/dev/i2c-0");

        imu.checkConnection();

        if (argc > 1)
        {
            const char * action = argv[1];
            if (0 == strcmp("raw", action))
            {
                streamRawValues(imu);
            }
            else if (0 == strcmp("gyro-only", action))
            {
                ahrs(imu, &fuse_gyro_only);
            }
            else if (0 == strcmp("compass-only", action))
            {
                ahrs(imu, &fuse_compass_only);
            }
            else
            {
                fprintf(stderr, "Unknown action '%s'.\n", action);
                return 3;
            }
        }
        else
        {
            ahrs(imu, &fuse_default);
        }
        return 0;
    }
    catch(const std::system_error & error)
    {
        std::string what = error.what();
        const std::error_code & code = error.code();
        std::cerr << "Error: " << what << "  " << code.message() << " (" << code << ")\n";
    }
    catch(const std::exception & error)
    {
        std::cerr << "Error: " << error.what() << '\n';
    }
    return 1;
}
<|endoftext|>


<commit_before>/*
 *  qi_roivolumes.cpp
 *
 *  Copyright (c) 2016 Tobias Wood.
 *
 *  This Source Code Form is subject to the terms of the Mozilla Public
 *  License, v. 2.0. If a copy of the MPL was not distributed with this
 *  file, You can obtain one at http://mozilla.org/MPL/2.0/.
 *
 */

#include <iostream>
#include <iomanip>
#include <fstream>
#include <string>

#include "Types.h"
#include "IO.h"
#include "Args.h"
#include "Util.h"

#include "itkLabelStatisticsImageFilter.h"
typedef itk::LabelStatisticsImageFilter<QI::VolumeF, QI::VolumeI> TStatsFilter;
typedef TStatsFilter::ValidLabelValuesContainerType TLabels;
// Declare arguments here so they are available in helper functions
args::ArgumentParser parser("Calculates average values or volumes of ROI labels.\n"
                            "If the --volumes flag is specified, only give the label images.\n"
                            "If --volumes is not specified, give label and value image pairs (all labels first, then all values).\n"
                            "Output goes to stdout\n"
                            "http://github.com/spinicist/QUIT");
args::PositionalList<std::string> in_paths(parser, "INPUT", "Input file paths.");
args::HelpFlag help(parser, "HELP", "Show this help menu", {'h', "help"});
args::Flag     verbose(parser, "VERBOSE", "Print more information (will mess up output, for test runs only)", {'v', "verbose"});
args::Flag     volumes(parser, "VOLUMES", "Output ROI volumes, not average values (does not require value images)", {'V', "volumes"});
args::ValueFlag<std::string> label_list_path(parser, "LABELS", "Specify labels and names to use in text file 'label number, label name' one per line", {'l', "labels"});
args::Flag     print_names(parser, "PRINT_NAMES", "Print label names in first column/row (LABEL_NUMBERS must be specified)", {'n', "print_names"});
args::Flag     transpose(parser, "TRANSPOSE", "Transpose output table (values go in rows instead of columns", {'t', "transpose"});
args::Flag     ignore_zero(parser, "IGNORE_ZERO", "Ignore 0 label (background)", {'z', "ignore_zero"});
args::Flag     sigma(parser, "SIGMA", "Print ±std along with mean", {'s', "sigma"});
args::ValueFlag<int> precision(parser, "PRECISION", "Number of decimal places (default 6)", {'p', "precision"}, 6);
args::ValueFlag<std::string> delim(parser, "DELIMITER", "Specify delimiter to use between entries (default ,)", {'d',"delim"}, ",");
args::ValueFlagList<std::string> header_paths(parser, "HEADER", "Add a header (can be specified multiple times)", {'H', "header"});
args::ValueFlagList<std::string> header_names(parser, "HEADER NAME", "Header name (must be specified in same order as paths)", {"header_name"});
args::ValueFlagList<double> scales(parser, "SCALE", "Divide ROI values by scale (must be same order as paths)", {"scale"});

/*
 * Helper function to work out the label list
 */
void GetLabelList(TLabels &label_numbers, std::vector<std::string> &label_names) {
    if (label_list_path) {
        if (verbose) std::cout << "Opening label list file: " << label_list_path.Get() << std::endl;
        std::ifstream file(label_list_path.Get());
        if (!file) {
            QI_EXCEPTION("Could not open label list file: " << label_list_path.Get());
        }
        std::string temp;
        while (std::getline(file, temp, ',')) {
            label_numbers.push_back(stoi(temp));
            std::getline(file, temp);
            temp.erase(std::remove(temp.begin(), temp.end(), '\r'), temp.end()); // Deal with rogue ^M characters
            label_names.push_back(temp);
            if (verbose) std::cout << "Read label: " << label_numbers.back() << ", name: " << label_names.back() << std::endl;
        }
    } else {
        if (verbose) std::cout << "Reading first label file to determine labels: " << QI::CheckList(in_paths).at(0) << std::endl;
        TStatsFilter::Pointer label_filter = TStatsFilter::New();
        QI::VolumeI::Pointer img = QI::ReadImage<QI::VolumeI>(QI::CheckList(in_paths).at(0));
        label_filter->SetLabelInput(img);
        label_filter->Update();
        label_numbers = label_filter->GetValidLabelValues();
        std::sort(label_numbers.begin(), label_numbers.end());
        if (verbose) {
            std::cout << "Found the following labels:" << std::endl;
            for (auto &l : label_numbers) std::cout << l << std::endl;
        }
    }
    if (ignore_zero) {
        if (verbose) std::cout << "Removing zero from label list." << std::endl;
        label_numbers.erase(std::remove(label_numbers.begin(), label_numbers.end(), 0), label_numbers.end());
    }
}

/*
 * Helper function to read in all the header lines
 */
std::vector<std::vector<std::string>> GetHeaders(int n_files) {
    if (header_names && (header_names.Get().size() != header_paths.Get().size())) {
        QI_EXCEPTION("Number of header names must match number of header paths");
    }
    std::vector<std::vector<std::string>> headers(header_paths.Get().size(), std::vector<std::string>());
    for (int h = 0; h < headers.size(); h++) {
        const std::string header_path = header_paths.Get().at(h);
        if (verbose) std::cout << "Reading header file: " << header_path << std::endl;
        std::ifstream header_file(header_path);
        if (!header_file) {
            QI_EXCEPTION("Failed to open header file: " << header_path);
        }
        std::string element;
        while (std::getline(header_file, element)) {
            headers.at(h).push_back(element);
        }
        if (headers.at(h).size() != n_files) {
            QI_EXCEPTION("Number of input files (" << n_files
                         << ") does not match number of entries (" << headers.at(h).size()
                         << ") in header: " << header_path)
        }
    }
    return headers;
}

/*
 * Helper function to actually work out all the values
 */
void GetValues(const int n_files, const TLabels &labels, const std::vector<double> &scale_list,
               std::vector<std::vector<double>> &mean_table, std::vector<std::vector<double>> &sigma_table, std::vector<std::vector<double>> &volume_table) {
    mean_table = std::vector<std::vector<double>>(n_files, std::vector<double>(labels.size()));
    sigma_table = std::vector<std::vector<double>>(n_files, std::vector<double>(labels.size()));
    volume_table = std::vector<std::vector<double>>(n_files, std::vector<double>(labels.size()));
    TStatsFilter::Pointer label_filter = TStatsFilter::New();
    QI::VolumeI::Pointer label_img = ITK_NULLPTR;
    QI::VolumeF::Pointer value_img = ITK_NULLPTR;
    for (int f = 0; f < n_files; f++) {
        if (volumes) {
            if (verbose) std::cout << "Reading label file: " << in_paths.Get().at(f) << std::endl;
            label_img = QI::ReadImage<QI::VolumeI>(in_paths.Get().at(f));
        } else {
            if (verbose) std::cout << "Reading label file: " << in_paths.Get().at(f) << std::endl;
            label_img = QI::ReadImage<QI::VolumeI>(in_paths.Get().at(f));
            if (verbose) std::cout << "Reading value file: " << in_paths.Get().at(f + n_files) << std::endl;
            value_img = QI::ReadImage(in_paths.Get().at(f + n_files));
            label_filter->SetInput(value_img);
        }
        double vox_volume = QI::VoxelVolume(label_img);
        label_filter->SetLabelInput(label_img);
        label_filter->Update();
        for (int i = 0; i < labels.size(); i++) {
            const double n_voxels = label_filter->GetCount(labels.at(i)); // This is the count of pixels
            mean_table.at(f).at(i) = label_filter->GetMean(labels.at(i)) / scale_list.at(f);
            sigma_table.at(f).at(i) = label_filter->GetSigma(labels.at(i)) / scale_list.at(f);
            volume_table.at(f).at(i) = label_filter->GetCount(labels.at(i)) * vox_volume;
        }
    }
}

/*
 * MAIN
 */
int main(int argc, char **argv) {
    QI::ParseArgs(parser, argc, argv);

    int n_files = QI::CheckList(in_paths).size();
    if (volumes) {
        if (verbose) std::cout << "There are " << n_files << " input images, finding ROI volumes" << std::endl;
    } else {
        // For ROI values, we have label and value image pairs
        if (n_files % 2 != 0) {
            std::cerr << "Require an even number of input images when finding ROI values" << std::endl;
            return EXIT_FAILURE;
        }
        n_files = n_files / 2;
        if (verbose) std::cout << "There are " << n_files << " input image pairs, finding mean ROI values" << std::endl;
    }
    // Setup label number list
    TLabels labels;
    std::vector<std::string> label_names;
    GetLabelList(labels, label_names);

    // Setup headers (if any)
    auto headers = GetHeaders(n_files);

    // Setup scales
    std::vector<double> scale_list = scales.Get();
    if (scale_list.size() < n_files) {
        int old_size = scale_list.size();
        scale_list.resize(n_files);
        for (int i = old_size; i < n_files; i++) {
            scale_list.at(i) = 1.0;
        }
    }
    if (verbose) {
        std::cout << "Scales are: ";
        for (const auto &s: scale_list) std::cout << s << " ";
        std::cout << std::endl;
    }

    // Now get the values/volumes
    std::vector<std::vector<double>> mean_table, sigma_table, volume_table;
    GetValues(n_files, labels, scale_list, mean_table, sigma_table, volume_table);

    if (verbose) std::cout << "Writing CSV: " << std::endl;
    if (precision) std::cout << std::fixed << std::setprecision(precision.Get());
    if (transpose) {
        if (print_names) {
            for (int h = 0; h < headers.size(); ++h) {
                if (header_names) {
                    std::cout << header_names.Get().at(h) << delim.Get();
                } else {
                    std::cout << header_paths.Get().at(h) << delim.Get();
                }
            }
            auto it = label_names.begin();
            std::cout << *it;
            for (++it; it != label_names.end(); ++it) {
                std::cout << delim.Get() << *it;
            }
            std::cout << std::endl;
        }
        for (int row = 0; row < n_files; ++row){
            for (auto h = headers.begin(); h != headers.end(); h++) {
                std::cout << h->at(row) << delim.Get();
            }
            if (volumes) {
                std::cout << volume_table.at(row).at(0);
            } else {
                std::cout << mean_table.at(row).at(0);
                if (sigma) std::cout << "±" << sigma_table.at(row).at(0);
            }
            for (int val = 1; val < labels.size(); ++val) {
                std::cout << delim.Get();
                if (volumes) {
                    std::cout << volume_table.at(row).at(val);
                } else {
                    std::cout << mean_table.at(row).at(val);
                    if (sigma) std::cout << "±" << sigma_table.at(row).at(val);
                }
            }
            std::cout << std::endl;
        }
    } else {
        for (int h = 0; h < headers.size(); h++) {
            if (print_names) {
                if (header_names) {
                    std::cout << header_names.Get().at(h) << delim.Get();
                } else {
                    std::cout << header_paths.Get().at(h) << delim.Get();
                }
            }
            auto h_el = headers.at(h).begin();
            std::cout << *h_el;
            for (++h_el; h_el != headers.at(h).end(); ++h_el) {
                std::cout << delim.Get() << *h_el;
            }
            std::cout << std::endl;
        }
        for (int l = 0; l < labels.size(); ++l) {
            if (print_names) {
                std::cout << label_names.at(l) << std::flush << delim.Get();
            }

            if (volumes) {
                std::cout << volume_table.at(0).at(l);
            } else {
                std::cout << mean_table.at(0).at(l);
                if (sigma) std::cout << "±" << sigma_table.at(0).at(l);
            }
            for (int file = 1; file < n_files; ++file) {
                std::cout << delim.Get();
                if (volumes) {
                    std::cout << volume_table.at(file).at(l);
                } else {
                    std::cout << mean_table.at(file).at(l);
                    if (sigma) std::cout << "±" << sigma_table.at(0).at(l);
                }
            }
            std::cout << std::endl;
        }
    }
    return EXIT_SUCCESS;
}

<commit_msg>BUG: Missing inputs for stats filter<commit_after>/*
 *  qi_roivolumes.cpp
 *
 *  Copyright (c) 2016 Tobias Wood.
 *
 *  This Source Code Form is subject to the terms of the Mozilla Public
 *  License, v. 2.0. If a copy of the MPL was not distributed with this
 *  file, You can obtain one at http://mozilla.org/MPL/2.0/.
 *
 */

#include <iostream>
#include <iomanip>
#include <fstream>
#include <string>

#include "Types.h"
#include "IO.h"
#include "Args.h"
#include "Util.h"

#include "itkLabelStatisticsImageFilter.h"
typedef itk::LabelStatisticsImageFilter<QI::VolumeF, QI::VolumeI> TStatsFilter;
typedef TStatsFilter::ValidLabelValuesContainerType TLabels;
// Declare arguments here so they are available in helper functions
args::ArgumentParser parser("Calculates average values or volumes of ROI labels.\n"
                            "If the --volumes flag is specified, only give the label images.\n"
                            "If --volumes is not specified, give label and value image pairs (all labels first, then all values).\n"
                            "Output goes to stdout\n"
                            "http://github.com/spinicist/QUIT");
args::PositionalList<std::string> in_paths(parser, "INPUT", "Input file paths.");
args::HelpFlag help(parser, "HELP", "Show this help menu", {'h', "help"});
args::Flag     verbose(parser, "VERBOSE", "Print more information (will mess up output, for test runs only)", {'v', "verbose"});
args::Flag     volumes(parser, "VOLUMES", "Output ROI volumes, not average values (does not require value images)", {'V', "volumes"});
args::ValueFlag<std::string> label_list_path(parser, "LABELS", "Specify labels and names to use in text file 'label number, label name' one per line", {'l', "labels"});
args::Flag     print_names(parser, "PRINT_NAMES", "Print label names in first column/row (LABEL_NUMBERS must be specified)", {'n', "print_names"});
args::Flag     transpose(parser, "TRANSPOSE", "Transpose output table (values go in rows instead of columns", {'t', "transpose"});
args::Flag     ignore_zero(parser, "IGNORE_ZERO", "Ignore 0 label (background)", {'z', "ignore_zero"});
args::Flag     sigma(parser, "SIGMA", "Print ±std along with mean", {'s', "sigma"});
args::ValueFlag<int> precision(parser, "PRECISION", "Number of decimal places (default 6)", {'p', "precision"}, 6);
args::ValueFlag<std::string> delim(parser, "DELIMITER", "Specify delimiter to use between entries (default ,)", {'d',"delim"}, ",");
args::ValueFlagList<std::string> header_paths(parser, "HEADER", "Add a header (can be specified multiple times)", {'H', "header"});
args::ValueFlagList<std::string> header_names(parser, "HEADER NAME", "Header name (must be specified in same order as paths)", {"header_name"});
args::ValueFlagList<double> scales(parser, "SCALE", "Divide ROI values by scale (must be same order as paths)", {"scale"});

/*
 * Helper function to work out the label list
 */
void GetLabelList(TLabels &label_numbers, std::vector<std::string> &label_names) {
    if (label_list_path) {
        if (verbose) std::cout << "Opening label list file: " << label_list_path.Get() << std::endl;
        std::ifstream file(label_list_path.Get());
        if (!file) {
            QI_EXCEPTION("Could not open label list file: " << label_list_path.Get());
        }
        std::string temp;
        while (std::getline(file, temp, ',')) {
            label_numbers.push_back(stoi(temp));
            std::getline(file, temp);
            temp.erase(std::remove(temp.begin(), temp.end(), '\r'), temp.end()); // Deal with rogue ^M characters
            label_names.push_back(temp);
            if (verbose) std::cout << "Read label: " << label_numbers.back() << ", name: " << label_names.back() << std::endl;
        }
    } else {
        TStatsFilter::Pointer label_filter = TStatsFilter::New();
        if (verbose) std::cout << "Reading first label file to determine labels: " << QI::CheckList(in_paths).at(0) << std::endl;
        label_filter->SetLabelInput(QI::ReadImage<QI::VolumeI>(QI::CheckList(in_paths).at(0)));
        label_filter->SetInput(QI::ReadImage<QI::VolumeF>(QI::CheckList(in_paths).at(0))); // Dummy value because ITK complains input primary is not set
        label_filter->Update();
        label_numbers = label_filter->GetValidLabelValues();
        std::sort(label_numbers.begin(), label_numbers.end());
        if (verbose) {
            std::cout << "Found the following labels:" << std::endl;
            for (auto &l : label_numbers) std::cout << l << " ";
            std::cout << std::endl;
        }
    }
    if (ignore_zero) {
        if (verbose) std::cout << "Removing zero from label list." << std::endl;
        label_numbers.erase(std::remove(label_numbers.begin(), label_numbers.end(), 0), label_numbers.end());
    }
}

/*
 * Helper function to read in all the header lines
 */
std::vector<std::vector<std::string>> GetHeaders(int n_files) {
    if (header_names && (header_names.Get().size() != header_paths.Get().size())) {
        QI_EXCEPTION("Number of header names must match number of header paths");
    }
    std::vector<std::vector<std::string>> headers(header_paths.Get().size(), std::vector<std::string>());
    for (int h = 0; h < headers.size(); h++) {
        const std::string header_path = header_paths.Get().at(h);
        if (verbose) std::cout << "Reading header file: " << header_path << std::endl;
        std::ifstream header_file(header_path);
        if (!header_file) {
            QI_EXCEPTION("Failed to open header file: " << header_path);
        }
        std::string element;
        while (std::getline(header_file, element)) {
            headers.at(h).push_back(element);
        }
        if (headers.at(h).size() != n_files) {
            QI_EXCEPTION("Number of input files (" << n_files
                         << ") does not match number of entries (" << headers.at(h).size()
                         << ") in header: " << header_path)
        }
    }
    return headers;
}

/*
 * Helper function to actually work out all the values
 */
void GetValues(const int n_files, const TLabels &labels, const std::vector<double> &scale_list,
               std::vector<std::vector<double>> &mean_table, std::vector<std::vector<double>> &sigma_table, std::vector<std::vector<double>> &volume_table) {
    mean_table = std::vector<std::vector<double>>(n_files, std::vector<double>(labels.size()));
    sigma_table = std::vector<std::vector<double>>(n_files, std::vector<double>(labels.size()));
    volume_table = std::vector<std::vector<double>>(n_files, std::vector<double>(labels.size()));
    TStatsFilter::Pointer label_filter = TStatsFilter::New();
    QI::VolumeI::Pointer label_img = ITK_NULLPTR;
    QI::VolumeF::Pointer value_img = ITK_NULLPTR;
    for (int f = 0; f < n_files; f++) {
        if (volumes) {
            if (verbose) std::cout << "Reading label file: " << in_paths.Get().at(f) << std::endl;
            label_img = QI::ReadImage<QI::VolumeI>(in_paths.Get().at(f));
            value_img = QI::ReadImage(in_paths.Get().at(f)); // Dummy image
        } else {
            if (verbose) std::cout << "Reading label file: " << in_paths.Get().at(f) << std::endl;
            label_img = QI::ReadImage<QI::VolumeI>(in_paths.Get().at(f));
            if (verbose) std::cout << "Reading value file: " << in_paths.Get().at(f + n_files) << std::endl;
            value_img = QI::ReadImage(in_paths.Get().at(f + n_files));
        }
        double vox_volume = QI::VoxelVolume(label_img);
        label_filter->SetLabelInput(label_img);
        label_filter->SetInput(value_img);
        label_filter->Update();
        for (int i = 0; i < labels.size(); i++) {
            mean_table.at(f).at(i) = label_filter->GetMean(labels.at(i)) / scale_list.at(f);
            sigma_table.at(f).at(i) = label_filter->GetSigma(labels.at(i)) / scale_list.at(f);
            volume_table.at(f).at(i) = label_filter->GetCount(labels.at(i)) * vox_volume;
        }
    }
}

/*
 * MAIN
 */
int main(int argc, char **argv) {
    QI::ParseArgs(parser, argc, argv);

    int n_files = QI::CheckList(in_paths).size();
    if (volumes) {
        if (verbose) std::cout << "There are " << n_files << " input images, finding ROI volumes" << std::endl;
    } else {
        // For ROI values, we have label and value image pairs
        if (n_files % 2 != 0) {
            std::cerr << "Require an even number of input images when finding ROI values" << std::endl;
            return EXIT_FAILURE;
        }
        n_files = n_files / 2;
        if (verbose) std::cout << "There are " << n_files << " input image pairs, finding mean ROI values" << std::endl;
    }
    // Setup label number list
    TLabels labels;
    std::vector<std::string> label_names;
    GetLabelList(labels, label_names);

    // Setup headers (if any)
    auto headers = GetHeaders(n_files);

    // Setup scales
    std::vector<double> scale_list = scales.Get();
    if (scale_list.size() < n_files) {
        int old_size = scale_list.size();
        scale_list.resize(n_files);
        for (int i = old_size; i < n_files; i++) {
            scale_list.at(i) = 1.0;
        }
    }
    if (verbose) {
        std::cout << "Scales are: ";
        for (const auto &s: scale_list) std::cout << s << " ";
        std::cout << std::endl;
    }

    // Now get the values/volumes
    std::vector<std::vector<double>> mean_table, sigma_table, volume_table;
    GetValues(n_files, labels, scale_list, mean_table, sigma_table, volume_table);

    if (verbose) std::cout << "Writing CSV: " << std::endl;
    if (precision) std::cout << std::fixed << std::setprecision(precision.Get());
    if (transpose) {
        if (print_names) {
            for (int h = 0; h < headers.size(); ++h) {
                if (header_names) {
                    std::cout << header_names.Get().at(h) << delim.Get();
                } else {
                    std::cout << header_paths.Get().at(h) << delim.Get();
                }
            }
            auto it = label_names.begin();
            std::cout << *it;
            for (++it; it != label_names.end(); ++it) {
                std::cout << delim.Get() << *it;
            }
            std::cout << std::endl;
        }
        for (int row = 0; row < n_files; ++row){
            for (auto h = headers.begin(); h != headers.end(); h++) {
                std::cout << h->at(row) << delim.Get();
            }
            if (volumes) {
                std::cout << volume_table.at(row).at(0);
            } else {
                std::cout << mean_table.at(row).at(0);
                if (sigma) std::cout << "±" << sigma_table.at(row).at(0);
            }
            for (int val = 1; val < labels.size(); ++val) {
                std::cout << delim.Get();
                if (volumes) {
                    std::cout << volume_table.at(row).at(val);
                } else {
                    std::cout << mean_table.at(row).at(val);
                    if (sigma) std::cout << "±" << sigma_table.at(row).at(val);
                }
            }
            std::cout << std::endl;
        }
    } else {
        for (int h = 0; h < headers.size(); h++) {
            if (print_names) {
                if (header_names) {
                    std::cout << header_names.Get().at(h) << delim.Get();
                } else {
                    std::cout << header_paths.Get().at(h) << delim.Get();
                }
            }
            auto h_el = headers.at(h).begin();
            std::cout << *h_el;
            for (++h_el; h_el != headers.at(h).end(); ++h_el) {
                std::cout << delim.Get() << *h_el;
            }
            std::cout << std::endl;
        }
        for (int l = 0; l < labels.size(); ++l) {
            if (print_names) {
                std::cout << label_names.at(l) << std::flush << delim.Get();
            }

            if (volumes) {
                std::cout << volume_table.at(0).at(l);
            } else {
                std::cout << mean_table.at(0).at(l);
                if (sigma) std::cout << "±" << sigma_table.at(0).at(l);
            }
            for (int file = 1; file < n_files; ++file) {
                std::cout << delim.Get();
                if (volumes) {
                    std::cout << volume_table.at(file).at(l);
                } else {
                    std::cout << mean_table.at(file).at(l);
                    if (sigma) std::cout << "±" << sigma_table.at(0).at(l);
                }
            }
            std::cout << std::endl;
        }
    }
    return EXIT_SUCCESS;
}

<|endoftext|>


<commit_before>#include "cmainwindow.h"
#include "ui_cmainwindow.h"

#include "settings/settings.h"
#include "settings/csettings.h"
#include "settingsui/csettingsdialog.h"
#include "settings/csettingspagecamera.h"

DISABLE_COMPILER_WARNINGS
#include <QCameraInfo>
#include <QDebug>
#include <QImageReader>
#include <QPainter>
#include <QTimer>
RESTORE_COMPILER_WARNINGS

#define PROBING_ENABLED_SETTING QStringLiteral("UI/ProbingEnabled")

CMainWindow::CMainWindow(QWidget *parent) :
	QMainWindow(parent),
	ui(new Ui::CMainWindow)
{
	ui->setupUi(this);
	ui->_displayWidget->installEventFilter(this);

	ui->actionProbing_enabled->setChecked(CSettings().value(PROBING_ENABLED_SETTING, true).toBool());
	connect(ui->actionProbing_enabled, &QAction::triggered, [](bool checked){
		CSettings().setValue(PROBING_ENABLED_SETTING, checked);
	});

	connect(&_frameGrabber, &ProxyVideoSurface::frameReceived, this, &CMainWindow::processFrame);

	connect(ui->actionConnect, &QAction::triggered, [this](bool connect){
		ui->actionConnect->setChecked(!connect);
		if (connect)
			startCamera();
		else
			stopCamera();
	});

	connect(ui->actionSettings, &QAction::triggered, [this](){
		CSettingsDialog(this)
			.addSettingsPage(new CSettingsPageCamera())
			.exec();
	});

	// Some streaming devices seem to malfunction if they are connected to too soon after Windows startup. This is a safeguard against that.
	QTimer::singleShot(7000, [this](){
		startCamera();
	});
}

CMainWindow::~CMainWindow()
{
	stopCamera();
	delete ui;
}

bool CMainWindow::eventFilter(QObject* /*object*/, QEvent* event)
{
	if (event->type() == QEvent::Paint)
	{
		QWidget * widget = ui->_displayWidget;
		QPainter painter(widget);
		// Fill the background according the the color assigned to this widget
		painter.fillRect(widget->rect(), painter.background());
		if (!_frame.isNull())
		{
			const QSize scaledImagesize = _frame.size().scaled(widget->size(), Qt::KeepAspectRatio);
			QRect imageDrawRect = widget->geometry();
			imageDrawRect.setSize(scaledImagesize);
			imageDrawRect.translate((widget->width() - scaledImagesize.width()) / 2, (widget->height() - scaledImagesize.height()) / 2);
			painter.drawImage(imageDrawRect, _frame);
		}

		if (_currentFrameContentsMetric != -1)
		{
			painter.setRenderHint(QPainter::TextAntialiasing, false);
			painter.setPen(QColor(0, 255, 255));
			painter.drawText(5, painter.fontMetrics().height()+5, QString::number(_currentFrameContentsMetric));
		}

		painter.end();
		return true;
	}

	return false;
}

void CMainWindow::startCamera()
{
	if (!_camera)
	{
		const auto cameras = QCameraInfo::availableCameras();
		for (const QCameraInfo& cameraInfo : cameras)
		{
			if (cameraInfo.isNull())
				continue;

			qDebug() << "Creating the camera" << cameraInfo.description();
			_camera = std::make_shared<QCamera>(cameraInfo);
			if (!_camera->isAvailable())
			{
				_camera.reset();
				continue;
			}

			_camera->setViewfinder(&_frameGrabber);

			connect(_camera.get(), &QCamera::stateChanged, [this](const QCamera::State state){
				if (state == QCamera::ActiveState)
				{
					ui->actionConnect->setChecked(true);
					setWindowTitle("Connected");
				}
				else
				{
					ui->actionConnect->setChecked(false);
					setWindowTitle("Not Connected");
				}
			});

			break;
		}
	}

	if (_camera)
	{
		qDebug() << "Connecting to the camera";

		_camera->load();
		qDebug() << "Supported resolutions:";
		QSize minSize {31000, 31000};
		for (const QSize& size: _camera->supportedViewfinderResolutions())
		{
			qDebug() << size;
			if (size.width() * size.height() < minSize.width() * minSize.height())
				minSize = size;
		}

		QCameraViewfinderSettings viewFinderSettings =_camera->viewfinderSettings();
		viewFinderSettings.setResolution(minSize);
		_camera->setViewfinderSettings(viewFinderSettings);
		_camera->start();
	}

	_frameScanFilter.reset();
}

void CMainWindow::stopCamera()
{
	if (_camera)
	{
		qDebug() << "Disconnecting from the camera";
		_camera->stop();
		_camera->unload();
		_frame = QImage();
		_currentFrameContentsMetric = -1;
		ui->_displayWidget->update();
	}

	setWindowTitle("Not Connected");
}

inline int analyzeFrame(const QImage& frame)
{
	if (frame.depth() != 32)
		return true;

	const int w = frame.width(), h = frame.height();
	const int sampleSquareSize = 20;
	const int sampleStrideW = w / sampleSquareSize, sampleStrideH = h / sampleSquareSize;
	uint64_t pixelsValueSum = 0;
	for (int y = 0; y < h; y += sampleStrideH)
	{
		const uint32_t* scanLine = (const uint32_t*) frame.scanLine(y);
		for (int x = 0; x < w; x += sampleStrideW)
		{
			// TODO: support non-32 bpp images
			// TODO: vectorization
			const uint32_t pixel = scanLine[y];
			pixelsValueSum += ((pixel & 0x00FF0000) >> 16) + ((pixel & 0x0000FF00) >> 8) + (pixel & 0x000000FF);
		}
	}

	const int result = (int) (pixelsValueSum / ((uint64_t) w / sampleStrideW * (uint64_t) h / sampleStrideH * 3ull));
	return result;
}

void CMainWindow::processFrame(QImage frame)
{
	if (!frame.isNull() && ui->actionProbing_enabled->isChecked())
	{
		const int threshold = CSettings().value(IMAGE_PIXEL_VALUE_THRESHOLD_SETTING, IMAGE_PIXEL_VALUE_THRESHOLD_DEFAULT).toInt();
		// This is the first frame upon connecting to the camera
		if (_frame.isNull())
		{
			_currentFrameContentsMetric = analyzeFrame(frame);
			const auto frameScanResult = _frameScanFilter.processSample(_currentFrameContentsMetric >= threshold);
			if (frameScanResult == Filter::Invalid)
			{
				// Disconnect and schedule re-check
				stopCamera();
				QTimer::singleShot(5000, [this](){
					startCamera();
				});

				return;
			}
			else if (frameScanResult == Filter::Undefined)
				return; // To avoid assigning _frame, which would prevent the following frames from being scanned
			else
			{
				// We've detected valid image!
				showFullScreen();
				raise();
			}
		}
		else // Not the first frame - apparently, we're streaming a live picture
			// Avoid checking every single frame
		{
			static uint32_t frameCounter = 0;
			++frameCounter;

			if (frameCounter > 30)
			{
				frameCounter = 0;
				_currentFrameContentsMetric = analyzeFrame(frame);
				if (_currentFrameContentsMetric < threshold)
				{
					// Disconnect and schedule re-check
					stopCamera();
					QTimer::singleShot(5000, [this](){
						startCamera();
					});

					showNormal();
					showMinimized();
					return;
				}
			}
		}
	}

	_frame = frame;
	ui->_displayWidget->update();
}
<commit_msg>Bug fix; cleanup<commit_after>#include "cmainwindow.h"
#include "ui_cmainwindow.h"

#include "settings/settings.h"
#include "settings/csettings.h"
#include "settingsui/csettingsdialog.h"
#include "settings/csettingspagecamera.h"

DISABLE_COMPILER_WARNINGS
#include <QCameraInfo>
#include <QDebug>
#include <QImageReader>
#include <QPainter>
#include <QTimer>
RESTORE_COMPILER_WARNINGS

#define PROBING_ENABLED_SETTING QStringLiteral("UI/ProbingEnabled")

CMainWindow::CMainWindow(QWidget *parent) :
	QMainWindow(parent),
	ui(new Ui::CMainWindow)
{
	ui->setupUi(this);
	ui->_displayWidget->installEventFilter(this);

	ui->actionProbing_enabled->setChecked(CSettings().value(PROBING_ENABLED_SETTING, true).toBool());
	connect(ui->actionProbing_enabled, &QAction::triggered, [](bool checked){
		CSettings().setValue(PROBING_ENABLED_SETTING, checked);
	});

	connect(&_frameGrabber, &ProxyVideoSurface::frameReceived, this, &CMainWindow::processFrame);

	connect(ui->actionConnect, &QAction::triggered, [this](bool connect){
		ui->actionConnect->setChecked(!connect);
		if (connect)
			startCamera();
		else
			stopCamera();
	});

	connect(ui->actionSettings, &QAction::triggered, [this](){
		CSettingsDialog(this)
			.addSettingsPage(new CSettingsPageCamera())
			.exec();
	});

	// Some streaming devices seem to malfunction if they are connected to too soon after Windows startup. This is a safeguard against that.
	QTimer::singleShot(7000, [this](){
		startCamera();
	});
}

CMainWindow::~CMainWindow()
{
	stopCamera();
	delete ui;
}

bool CMainWindow::eventFilter(QObject* /*object*/, QEvent* event)
{
	if (event->type() == QEvent::Paint)
	{
		QWidget * widget = ui->_displayWidget;
		QPainter painter(widget);
		// Fill the background according the the color assigned to this widget
		painter.fillRect(widget->rect(), painter.background());
		if (!_frame.isNull())
		{
			const QSize scaledImagesize = _frame.size().scaled(widget->size(), Qt::KeepAspectRatio);
			QRect imageDrawRect = widget->geometry();
			imageDrawRect.setSize(scaledImagesize);
			imageDrawRect.translate((widget->width() - scaledImagesize.width()) / 2, (widget->height() - scaledImagesize.height()) / 2);
			painter.drawImage(imageDrawRect, _frame);
		}

		if (_currentFrameContentsMetric != -1)
		{
			painter.setRenderHint(QPainter::TextAntialiasing, false);
			painter.setPen(QColor(255, 0, 255));
			painter.drawText(5, painter.fontMetrics().height()+5, QString::number(_currentFrameContentsMetric));
		}

		painter.end();
		return true;
	}

	return false;
}

void CMainWindow::startCamera()
{
	if (!_camera)
	{
		const auto cameras = QCameraInfo::availableCameras();
		for (const QCameraInfo& cameraInfo : cameras)
		{
			if (cameraInfo.isNull())
				continue;

			qDebug() << "Creating the camera" << cameraInfo.description();
			_camera = std::make_shared<QCamera>(cameraInfo);
			if (!_camera->isAvailable())
			{
				_camera.reset();
				continue;
			}

			_camera->setViewfinder(&_frameGrabber);

			connect(_camera.get(), &QCamera::stateChanged, [this](const QCamera::State state){
					ui->actionConnect->setChecked(state == QCamera::ActiveState);
			});

			break;
		}
	}

	if (_camera)
	{
		qDebug() << "Connecting to the camera";

		_camera->load();
		qDebug() << "Supported resolutions:";
		QSize minSize {31000, 31000};
		for (const QSize& size: _camera->supportedViewfinderResolutions())
		{
			qDebug() << size;
			if (size.width() * size.height() < minSize.width() * minSize.height())
				minSize = size;
		}

		QCameraViewfinderSettings viewFinderSettings =_camera->viewfinderSettings();
		viewFinderSettings.setResolution(minSize);
		_camera->setViewfinderSettings(viewFinderSettings);
		_camera->start();
	}

	_frameScanFilter.reset();
}

void CMainWindow::stopCamera()
{
	if (_camera)
	{
		qDebug() << "Disconnecting from the camera";
		_camera->stop();
		_camera->unload();
		_frame = QImage();
		_currentFrameContentsMetric = -1;
		ui->_displayWidget->update();
	}

	showNormal();
}

inline int analyzeFrame(const QImage& frame)
{
	if (frame.depth() != 32)
		return true;

	const int w = frame.width(), h = frame.height();
	const int sampleSquareSize = 20;
	const int sampleStrideW = w / sampleSquareSize, sampleStrideH = h / sampleSquareSize;
	uint64_t pixelsValueSum = 0;
	for (int y = 0; y < h; y += sampleStrideH)
	{
		const uint32_t* scanLine = (const uint32_t*) frame.scanLine(y);
		for (int x = 0; x < w; x += sampleStrideW)
		{
			// TODO: support non-32 bpp images
			// TODO: vectorization
			const uint32_t pixel = scanLine[y];
			pixelsValueSum += ((pixel & 0x00FF0000) >> 16) + ((pixel & 0x0000FF00) >> 8) + (pixel & 0x000000FF);
		}
	}

	const int result = (int) (pixelsValueSum / ((uint64_t) w / sampleStrideW * (uint64_t) h / sampleStrideH * 3ull));
	return result;
}

void CMainWindow::processFrame(QImage frame)
{
	if (!frame.isNull() && ui->actionProbing_enabled->isChecked())
	{
		const int threshold = CSettings().value(IMAGE_PIXEL_VALUE_THRESHOLD_SETTING, IMAGE_PIXEL_VALUE_THRESHOLD_DEFAULT).toInt();
		// This is the first frame upon connecting to the camera
		if (_frame.isNull())
		{
			_currentFrameContentsMetric = analyzeFrame(frame);
			const auto frameScanResult = _frameScanFilter.processSample(_currentFrameContentsMetric >= threshold);
			if (frameScanResult == Filter::Invalid)
			{
				// Disconnect and schedule re-check
				stopCamera();
				QTimer::singleShot(5000, [this](){
					startCamera();
				});

				return;
			}
			else if (frameScanResult == Filter::Undefined)
				return; // To avoid assigning _frame, which would prevent the following frames from being scanned
			else
			{
				// We've detected valid image!
				showNormal(); // showFullScreen doesn't work properly if the window was minimized
				showFullScreen();
				raise();
			}
		}
		else // Not the first frame - apparently, we're streaming a live picture
			// Avoid checking every single frame
		{
			static uint32_t frameCounter = 0;
			++frameCounter;

			if (frameCounter > 30)
			{
				frameCounter = 0;
				_currentFrameContentsMetric = analyzeFrame(frame);
				if (_currentFrameContentsMetric < threshold)
				{
					// Disconnect and schedule re-check
					stopCamera();
					QTimer::singleShot(5000, [this](){
						startCamera();
					});

					showMinimized();
					return;
				}
			}
		}
	}

	_frame = frame;
	ui->_displayWidget->update();
}
<|endoftext|>


<commit_before>#include "RaceModeDashboardView.h"
#include "../../PresenterLayer/BatteryPresenter/BatteryPresenter.h"
#include "../../PresenterLayer/AuxBmsPresenter/AuxBmsPresenter.h"
#include "../../PresenterLayer/DriverControlsPresenter/DriverControlsPresenter.h"
#include "../../PresenterLayer/KeyMotorPresenter/KeyMotorPresenter.h"
#include "../../PresenterLayer/LightsPresenter/LightsPresenter.h"
#include "../../PresenterLayer/MpptPresenter/MpptPresenter.h"
#include "../../PresenterLayer/MotorDetailsPresenter/MotorDetailsPresenter.h"
#include "../../PresenterLayer/MotorFaultsPresenter/MotorFaultsPresenter.h"
#include "../RaceModeDisplay/RaceModeDisplayUI/I_RaceModeDashboardUI.h"

namespace
{
    const double RED_SLOPE = -1;
    const int RED_INITIAL = 220;
    const double GREEN_SLOPE = 1.7;
    const int GREEN_INITIAL = 10;
    const int BLUE_INITIAL = 20;
    const QString DEFAULT_STYLESHEET = "QProgressBar:horizontal {\
            border: 1px solid white;\
            border-radius: 7px;\
            background: black;\
            }\
            QProgressBar::chunk:horizontal{\
            border-radius: 7px;\
            background: ";
    const QString TEMPERATURE_UNIT = "o";
}

RaceModeDashboardView::RaceModeDashboardView(BatteryPresenter& batteryPresenter,
        BatteryFaultsPresenter& batteryFaultsPresenter,
        AuxBmsPresenter& auxBmsPresenter,
        DriverControlsPresenter& driverControlsPresenter,
        KeyMotorPresenter& keyMotorPresenter,
        LightsPresenter& lightsPresenter,
        MpptPresenter& mpptPresenter,
        MotorDetailsPresenter& motorDetailsPresenter,
        MotorFaultsPresenter& motorFaultsPresenter,
        I_RaceModeDashboardUI& ui,
        MotorFaultList& motorZeroFaultsList,
        MotorFaultList& motorOneFaultsList,
        BatteryFaultList& batteryFaultsList)
    : batteryPresenter_(batteryPresenter)
    , batteryFaultsPresenter_(batteryFaultsPresenter)
    , auxBmsPresenter_(auxBmsPresenter)
    , driverControlsPresenter_(driverControlsPresenter)
    , keyMotorPresenter_(keyMotorPresenter)
    , lightsPresenter_(lightsPresenter)
    , mpptPresenter_(mpptPresenter)
    , motorDetailsPresenter_(motorDetailsPresenter)
    , motorFaultsPresenter_(motorFaultsPresenter)
    , ui_(ui)
    , motorZeroFaultsList_(motorZeroFaultsList)
    , motorOneFaultsList_(motorOneFaultsList)
    , batteryFaultsList_(batteryFaultsList)
{
    connectBattery(batteryPresenter_);
    connectBatteryFaults(batteryFaultsPresenter_);
    connectAuxBms(auxBmsPresenter_);
    connectDriverControls(driverControlsPresenter_);
    connectKeyMotor(keyMotorPresenter_);
    connectLights(lightsPresenter_);
    connectMppt(mpptPresenter_);
    connectMotorDetails(motorDetailsPresenter_);
    connectMotorFaults(motorFaultsPresenter_);
    //ui_.showMaximized();
    ui_.show();
}
RaceModeDashboardView::~RaceModeDashboardView()
{
}

void RaceModeDashboardView::connectBattery(BatteryPresenter& batteryPresenter)
{
    connect(&batteryPresenter, SIGNAL(aliveReceived(bool)),
            this, SLOT(aliveReceived(bool)));
    connect(&batteryPresenter, SIGNAL(packNetPowerReceived(double)),
            this, SLOT(packNetPowerReceived(double)));
    connect(&batteryPresenter, SIGNAL(packStateOfChargeReceived(double)),
            this, SLOT(packStateOfChargeReceived(double)));
    connect(&batteryPresenter, SIGNAL(lowCellVoltageReceived(int)),
            this, SLOT(lowCellVoltageReceived(int)));
    connect(&batteryPresenter, SIGNAL(averageCellVoltageReceived(int)),
            this, SLOT(averageCellVoltageReceived(int)));
    connect(&batteryPresenter, SIGNAL(highTemperatureReceived(int)),
            this, SLOT(highTemperatureReceived(int)));
    connect(&batteryPresenter, SIGNAL(averageTemperatureReceived(int)),
            this, SLOT(averageTemperatureReceived(int)));
}

void RaceModeDashboardView::connectBatteryFaults(BatteryFaultsPresenter& batteryFaultsPresenter)
{
    connect(&batteryFaultsPresenter, SIGNAL(errorFlagsReceived(BatteryErrorFlags)),
            this, SLOT(errorFlagsReceived(BatteryErrorFlags)));
    connect(&batteryFaultsPresenter, SIGNAL(limitFlagsReceived(BatteryLimitFlags)),
            this, SLOT(limitFlagsReceived(BatteryLimitFlags)));
}

void RaceModeDashboardView::connectAuxBms(AuxBmsPresenter& auxBmsPresenter)
{
    connect(&auxBmsPresenter, SIGNAL(prechargeStateReceived(QString)),
            this, SLOT(prechargeStateReceived(QString)));
    connect(&auxBmsPresenter, SIGNAL(auxVoltageReceived(int)),
            this, SLOT(auxVoltageReceived(int)));
}

void RaceModeDashboardView::connectDriverControls(DriverControlsPresenter& driverControlsPresenter)
{
    connect(&driverControlsPresenter, SIGNAL(resetReceived(bool)),
            this, SLOT(resetReceived(bool)));
}

void RaceModeDashboardView::connectKeyMotor(KeyMotorPresenter& keyMotorPresenter)
{
    connect(&keyMotorPresenter, SIGNAL(motorSetCurrentReceived(double)),
            this, SLOT(motorSetCurrentReceived(double)));
    connect(&keyMotorPresenter, SIGNAL(motorActualSpeedReceived(double)),
            this, SLOT(motorActualSpeedReceived(double)));
    connect(&keyMotorPresenter, SIGNAL(motorBusVoltageReceived(double)),
            this, SLOT(motorBusVoltageReceived(double)));
    connect(&keyMotorPresenter, SIGNAL(motorBusCurrentReceived(double)),
            this, SLOT(motorBusCurrentReceived(double)));
}

void RaceModeDashboardView::connectLights(LightsPresenter& lightsPresenter)
{
    connect(&lightsPresenter, SIGNAL(lowBeamsReceived(bool)),
            this, SLOT(lowBeamsReceived(bool)));
    connect(&lightsPresenter, SIGNAL(highBeamsReceived(bool)),
            this, SLOT(highBeamsReceived(bool)));
    connect(&lightsPresenter, SIGNAL(leftSignalReceived(bool)),
            this, SLOT(leftSignalReceived(bool)));
    connect(&lightsPresenter, SIGNAL(rightSignalReceived(bool)),
            this, SLOT(rightSignalReceived(bool)));
    connect(&lightsPresenter, SIGNAL(lightAliveReceived(bool)),
            this, SLOT(lightAliveReceived(bool)));
}

void RaceModeDashboardView::connectMppt(MpptPresenter& mpptPresenter)
{
    connect(&mpptPresenter, SIGNAL(mpptReceived(int, Mppt)),
            this, SLOT(mpptReceived(int, Mppt)));
    connect(&mpptPresenter, SIGNAL(mpptPowerReceived(double)),
            this, SLOT(mpptPowerReceived(double)));
}

void RaceModeDashboardView::connectMotorDetails(MotorDetailsPresenter& motorDetailsPresenter)
{
}

void RaceModeDashboardView::connectMotorFaults(MotorFaultsPresenter& motorFaultsPresenter)
{
    connect(&motorFaultsPresenter, SIGNAL(motorZeroErrorFlagsReceived(ErrorFlags)),
            this, SLOT(motorZeroErrorFlagsReceived(ErrorFlags)));
    connect(&motorFaultsPresenter, SIGNAL(motorZeroLimitFlagsReceived(LimitFlags)),
            this, SLOT(motorZeroLimitFlagsReceived(LimitFlags)));
    connect(&motorFaultsPresenter, SIGNAL(motorOneErrorFlagsReceived(ErrorFlags)),
            this, SLOT(motorOneErrorFlagsReceived(ErrorFlags)));
    connect(&motorFaultsPresenter, SIGNAL(motorOneLimitFlagsReceived(LimitFlags)),
            this, SLOT(motorOneLimitFlagsReceived(LimitFlags)));
}

void RaceModeDashboardView::updateFaultLabel(QLabel& dashboardLabel, FaultLabel faultLabel)
{
    if (faultLabel.priority() >= 0)
    {
        dashboardLabel.setStyleSheet(QString("font: 10pt \"Burlingame Pro\";\n color:%1").arg(faultLabel.color().name()));
        dashboardLabel.setText(faultLabel.text());
    }
    else
    {
        dashboardLabel.setText("");
    }
}

void RaceModeDashboardView::aliveReceived(bool)
{
}

void RaceModeDashboardView::prechargeStateReceived(QString prechargeState)
{
    ui_.prechargeStateLabel().setText(prechargeState);
}

void RaceModeDashboardView::packNetPowerReceived(double netPower)
{
    ui_.netPowerLabel().setText(QString::number(netPower, 'f', 1));
    ui_.powerOutLabel().setText(QString::number(qAbs(netPower - ui_.powerInLabel().text().toDouble()), 'f', 1));
}

void RaceModeDashboardView::auxVoltageReceived(int auxVoltage)
{
    ui_.auxVoltageLabel().setText(QString::number(auxVoltage, 'f', 2));
}

void RaceModeDashboardView::packStateOfChargeReceived(double stateOfCharge)
{
    ui_.stateOfChargeCapacityWidget().setValue(stateOfCharge);
}

void RaceModeDashboardView::lowCellVoltageReceived(int lowVoltage)
{
    ui_.lowestCellVoltageLabel().setNum(lowVoltage);
}

void RaceModeDashboardView::averageCellVoltageReceived(int averageVoltage)
{
    ui_.avgCellVoltageLabel().setNum(averageVoltage);
}

void RaceModeDashboardView::highTemperatureReceived(int highTemp)
{
    ui_.maxCellTemperatureLabel().setText(QString::number(highTemp) + " " + TEMPERATURE_UNIT);
}

void RaceModeDashboardView::averageTemperatureReceived(int avgTemp)
{
    ui_.avgCellTemperatureLabel().setText(QString::number(avgTemp) + " " + TEMPERATURE_UNIT);
}

void RaceModeDashboardView::errorFlagsReceived(BatteryErrorFlags batteryErrorFlags)
{
    batteryFaultsList_.updateErrors(batteryErrorFlags);
    updateFaultLabel(ui_.batteryFaultsLabel(), batteryFaultsList_.getHighestActivePriorityLabel());
}
void RaceModeDashboardView::limitFlagsReceived(BatteryLimitFlags batteryLimitFlags)
{
    batteryFaultsList_.updateLimits(batteryLimitFlags);
    updateFaultLabel(ui_.batteryFaultsLabel(), batteryFaultsList_.getHighestActivePriorityLabel());
}
void RaceModeDashboardView::resetReceived(bool reset)
{
    if (reset)
    {
        ui_.motorResetButtonWidget().setStyleSheet("background: url(:/Resources/MotorResetButton.png)");
    }
    else
    {
        ui_.motorResetButtonWidget().setStyleSheet("");
    }
}
void RaceModeDashboardView::motorSetCurrentReceived(double setCurrent)
{
    ui_.setCurrentLabel().setText(QString::number(setCurrent, 'f', 3));
}
void RaceModeDashboardView::motorActualSpeedReceived(double actualSpeed)
{
    ui_.actualSpeedLabel().setText(QString::number(actualSpeed, 'f', 1));
}
void RaceModeDashboardView::motorBusVoltageReceived(double busVoltage)
{
    ui_.busVoltageLabel().setText(QString::number(busVoltage, 'f', 2));
    busVoltage_ = busVoltage;
    setMotorPower();
}
void RaceModeDashboardView::motorBusCurrentReceived(double busCurrent)
{
    ui_.busCurrentLabel().setText(QString::number(busCurrent, 'f', 3));
    busCurrent_ = busCurrent;
    setMotorPower();
}

void RaceModeDashboardView::setMotorPower()
{
    ui_.motorPowerLabel().setText(QString::number((busVoltage_ * busCurrent_), 'f', 2));
}

void RaceModeDashboardView::lowBeamsReceived(bool lowBeams)
{
    if (lowBeams)
    {
        ui_.lowHeadlightIndicatorWidget().setStyleSheet("border-image: url(:/Resources/LowHeadlightIndicator.png) 0 0 0 0 stretch stretch");
    }
    else
    {
        ui_.lowHeadlightIndicatorWidget().setStyleSheet("");
    }
}
void RaceModeDashboardView::highBeamsReceived(bool highBeams)
{
    if (highBeams)
    {
        ui_.highHeadlightIndicatorWidget().setStyleSheet("border-image: url(:/Resources/HighHeadlightIndicator.png) 0 0 0 0 stretch stretch");
    }
    else
    {
        ui_.highHeadlightIndicatorWidget().setStyleSheet("");
    }
}
void RaceModeDashboardView::leftSignalReceived(bool leftSignal)
{
    if (leftSignal)
    {
        ui_.leftTurnSignalWidget().setStyleSheet("border-image: url(:/Resources/TurnSignalLeft.png) 0 0 0 0 stretch stretch;");
    }
    else
    {
        ui_.leftTurnSignalWidget().setStyleSheet("");
    }
}
void RaceModeDashboardView::rightSignalReceived(bool rightSignal)
{
    if (rightSignal)
    {
        ui_.rightTurnSignalWidget().setStyleSheet("border-image: url(:/Resources/TurnSignalRight.png) 0 0 0 0 stretch stretch;");
    }
    else
    {
        ui_.rightTurnSignalWidget().setStyleSheet("");
    }
}
void RaceModeDashboardView::lightAliveReceived(bool)
{

}

void RaceModeDashboardView::mpptReceived(int i, Mppt mppt)
{
    if (i == 0)
    {
        mpptZeroPower_ = mppt.arrayCurrent() * mppt.arrayVoltage();
    }

    else if (i == 1)
    {
        mpptOnePower_ = mppt.arrayCurrent() * mppt.arrayVoltage();
    }

    else if (i == 2)
    {
        mpptTwoPower_ = mppt.arrayCurrent() * mppt.arrayVoltage();
    }
    else if (i == 3)
    {
        mpptThreePower_ = mppt.arrayCurrent() * mppt.arrayVoltage();
    }

    mpptPowerReceived(mpptZeroPower_ + mpptOnePower_ + mpptTwoPower_ + mpptThreePower_);
}

void RaceModeDashboardView::mpptPowerReceived(double mpptPower)
{
    ui_.powerInLabel().setText(QString::number(mpptPower, 'f', 1));
    ui_.powerOutLabel().setText(QString::number(qAbs(ui_.netPowerLabel().text().toDouble() - mpptPower), 'f', 1));
}

void RaceModeDashboardView::motorZeroErrorFlagsReceived(ErrorFlags flags)
{
    motorZeroFaultsList_.updateErrors(flags);
    updateFaultLabel(ui_.motorZeroFaultsLabel(), motorZeroFaultsList_.getHighestActivePriorityLabel());
}

void RaceModeDashboardView::motorZeroLimitFlagsReceived(LimitFlags flags)
{
    motorZeroFaultsList_.updateLimits(flags);
    updateFaultLabel(ui_.motorZeroFaultsLabel(), motorZeroFaultsList_.getHighestActivePriorityLabel());
}

void RaceModeDashboardView::motorOneErrorFlagsReceived(ErrorFlags flags)
{
    motorOneFaultsList_.updateErrors(flags);
    updateFaultLabel(ui_.motorOneFaultsLabel(), motorOneFaultsList_.getHighestActivePriorityLabel());
}

void RaceModeDashboardView::motorOneLimitFlagsReceived(LimitFlags flags)
{
    motorOneFaultsList_.updateLimits(flags);
    updateFaultLabel(ui_.motorOneFaultsLabel(), motorOneFaultsList_.getHighestActivePriorityLabel());
}
<commit_msg>Fixed rounding error for Bus Current<commit_after>#include "RaceModeDashboardView.h"
#include "../../PresenterLayer/BatteryPresenter/BatteryPresenter.h"
#include "../../PresenterLayer/AuxBmsPresenter/AuxBmsPresenter.h"
#include "../../PresenterLayer/DriverControlsPresenter/DriverControlsPresenter.h"
#include "../../PresenterLayer/KeyMotorPresenter/KeyMotorPresenter.h"
#include "../../PresenterLayer/LightsPresenter/LightsPresenter.h"
#include "../../PresenterLayer/MpptPresenter/MpptPresenter.h"
#include "../../PresenterLayer/MotorDetailsPresenter/MotorDetailsPresenter.h"
#include "../../PresenterLayer/MotorFaultsPresenter/MotorFaultsPresenter.h"
#include "../RaceModeDisplay/RaceModeDisplayUI/I_RaceModeDashboardUI.h"

namespace
{
    const double RED_SLOPE = -1;
    const int RED_INITIAL = 220;
    const double GREEN_SLOPE = 1.7;
    const int GREEN_INITIAL = 10;
    const int BLUE_INITIAL = 20;
    const QString DEFAULT_STYLESHEET = "QProgressBar:horizontal {\
            border: 1px solid white;\
            border-radius: 7px;\
            background: black;\
            }\
            QProgressBar::chunk:horizontal{\
            border-radius: 7px;\
            background: ";
    const QString TEMPERATURE_UNIT = "o";
}

RaceModeDashboardView::RaceModeDashboardView(BatteryPresenter& batteryPresenter,
        BatteryFaultsPresenter& batteryFaultsPresenter,
        AuxBmsPresenter& auxBmsPresenter,
        DriverControlsPresenter& driverControlsPresenter,
        KeyMotorPresenter& keyMotorPresenter,
        LightsPresenter& lightsPresenter,
        MpptPresenter& mpptPresenter,
        MotorDetailsPresenter& motorDetailsPresenter,
        MotorFaultsPresenter& motorFaultsPresenter,
        I_RaceModeDashboardUI& ui,
        MotorFaultList& motorZeroFaultsList,
        MotorFaultList& motorOneFaultsList,
        BatteryFaultList& batteryFaultsList)
    : batteryPresenter_(batteryPresenter)
    , batteryFaultsPresenter_(batteryFaultsPresenter)
    , auxBmsPresenter_(auxBmsPresenter)
    , driverControlsPresenter_(driverControlsPresenter)
    , keyMotorPresenter_(keyMotorPresenter)
    , lightsPresenter_(lightsPresenter)
    , mpptPresenter_(mpptPresenter)
    , motorDetailsPresenter_(motorDetailsPresenter)
    , motorFaultsPresenter_(motorFaultsPresenter)
    , ui_(ui)
    , motorZeroFaultsList_(motorZeroFaultsList)
    , motorOneFaultsList_(motorOneFaultsList)
    , batteryFaultsList_(batteryFaultsList)
{
    connectBattery(batteryPresenter_);
    connectBatteryFaults(batteryFaultsPresenter_);
    connectAuxBms(auxBmsPresenter_);
    connectDriverControls(driverControlsPresenter_);
    connectKeyMotor(keyMotorPresenter_);
    connectLights(lightsPresenter_);
    connectMppt(mpptPresenter_);
    connectMotorDetails(motorDetailsPresenter_);
    connectMotorFaults(motorFaultsPresenter_);
    //ui_.showMaximized();
    ui_.show();
}
RaceModeDashboardView::~RaceModeDashboardView()
{
}

void RaceModeDashboardView::connectBattery(BatteryPresenter& batteryPresenter)
{
    connect(&batteryPresenter, SIGNAL(aliveReceived(bool)),
            this, SLOT(aliveReceived(bool)));
    connect(&batteryPresenter, SIGNAL(packNetPowerReceived(double)),
            this, SLOT(packNetPowerReceived(double)));
    connect(&batteryPresenter, SIGNAL(packStateOfChargeReceived(double)),
            this, SLOT(packStateOfChargeReceived(double)));
    connect(&batteryPresenter, SIGNAL(lowCellVoltageReceived(int)),
            this, SLOT(lowCellVoltageReceived(int)));
    connect(&batteryPresenter, SIGNAL(averageCellVoltageReceived(int)),
            this, SLOT(averageCellVoltageReceived(int)));
    connect(&batteryPresenter, SIGNAL(highTemperatureReceived(int)),
            this, SLOT(highTemperatureReceived(int)));
    connect(&batteryPresenter, SIGNAL(averageTemperatureReceived(int)),
            this, SLOT(averageTemperatureReceived(int)));
}

void RaceModeDashboardView::connectBatteryFaults(BatteryFaultsPresenter& batteryFaultsPresenter)
{
    connect(&batteryFaultsPresenter, SIGNAL(errorFlagsReceived(BatteryErrorFlags)),
            this, SLOT(errorFlagsReceived(BatteryErrorFlags)));
    connect(&batteryFaultsPresenter, SIGNAL(limitFlagsReceived(BatteryLimitFlags)),
            this, SLOT(limitFlagsReceived(BatteryLimitFlags)));
}

void RaceModeDashboardView::connectAuxBms(AuxBmsPresenter& auxBmsPresenter)
{
    connect(&auxBmsPresenter, SIGNAL(prechargeStateReceived(QString)),
            this, SLOT(prechargeStateReceived(QString)));
    connect(&auxBmsPresenter, SIGNAL(auxVoltageReceived(int)),
            this, SLOT(auxVoltageReceived(int)));
}

void RaceModeDashboardView::connectDriverControls(DriverControlsPresenter& driverControlsPresenter)
{
    connect(&driverControlsPresenter, SIGNAL(resetReceived(bool)),
            this, SLOT(resetReceived(bool)));
}

void RaceModeDashboardView::connectKeyMotor(KeyMotorPresenter& keyMotorPresenter)
{
    connect(&keyMotorPresenter, SIGNAL(motorSetCurrentReceived(double)),
            this, SLOT(motorSetCurrentReceived(double)));
    connect(&keyMotorPresenter, SIGNAL(motorActualSpeedReceived(double)),
            this, SLOT(motorActualSpeedReceived(double)));
    connect(&keyMotorPresenter, SIGNAL(motorBusVoltageReceived(double)),
            this, SLOT(motorBusVoltageReceived(double)));
    connect(&keyMotorPresenter, SIGNAL(motorBusCurrentReceived(double)),
            this, SLOT(motorBusCurrentReceived(double)));
}

void RaceModeDashboardView::connectLights(LightsPresenter& lightsPresenter)
{
    connect(&lightsPresenter, SIGNAL(lowBeamsReceived(bool)),
            this, SLOT(lowBeamsReceived(bool)));
    connect(&lightsPresenter, SIGNAL(highBeamsReceived(bool)),
            this, SLOT(highBeamsReceived(bool)));
    connect(&lightsPresenter, SIGNAL(leftSignalReceived(bool)),
            this, SLOT(leftSignalReceived(bool)));
    connect(&lightsPresenter, SIGNAL(rightSignalReceived(bool)),
            this, SLOT(rightSignalReceived(bool)));
    connect(&lightsPresenter, SIGNAL(lightAliveReceived(bool)),
            this, SLOT(lightAliveReceived(bool)));
}

void RaceModeDashboardView::connectMppt(MpptPresenter& mpptPresenter)
{
    connect(&mpptPresenter, SIGNAL(mpptReceived(int, Mppt)),
            this, SLOT(mpptReceived(int, Mppt)));
    connect(&mpptPresenter, SIGNAL(mpptPowerReceived(double)),
            this, SLOT(mpptPowerReceived(double)));
}

void RaceModeDashboardView::connectMotorDetails(MotorDetailsPresenter& motorDetailsPresenter)
{
}

void RaceModeDashboardView::connectMotorFaults(MotorFaultsPresenter& motorFaultsPresenter)
{
    connect(&motorFaultsPresenter, SIGNAL(motorZeroErrorFlagsReceived(ErrorFlags)),
            this, SLOT(motorZeroErrorFlagsReceived(ErrorFlags)));
    connect(&motorFaultsPresenter, SIGNAL(motorZeroLimitFlagsReceived(LimitFlags)),
            this, SLOT(motorZeroLimitFlagsReceived(LimitFlags)));
    connect(&motorFaultsPresenter, SIGNAL(motorOneErrorFlagsReceived(ErrorFlags)),
            this, SLOT(motorOneErrorFlagsReceived(ErrorFlags)));
    connect(&motorFaultsPresenter, SIGNAL(motorOneLimitFlagsReceived(LimitFlags)),
            this, SLOT(motorOneLimitFlagsReceived(LimitFlags)));
}

void RaceModeDashboardView::updateFaultLabel(QLabel& dashboardLabel, FaultLabel faultLabel)
{
    if (faultLabel.priority() >= 0)
    {
        dashboardLabel.setStyleSheet(QString("font: 10pt \"Burlingame Pro\";\n color:%1").arg(faultLabel.color().name()));
        dashboardLabel.setText(faultLabel.text());
    }
    else
    {
        dashboardLabel.setText("");
    }
}

void RaceModeDashboardView::aliveReceived(bool)
{
}

void RaceModeDashboardView::prechargeStateReceived(QString prechargeState)
{
    ui_.prechargeStateLabel().setText(prechargeState);
}

void RaceModeDashboardView::packNetPowerReceived(double netPower)
{
    ui_.netPowerLabel().setText(QString::number(netPower, 'f', 1));
    ui_.powerOutLabel().setText(QString::number(qAbs(netPower - ui_.powerInLabel().text().toDouble()), 'f', 1));
}

void RaceModeDashboardView::auxVoltageReceived(int auxVoltage)
{
    ui_.auxVoltageLabel().setText(QString::number(auxVoltage, 'f', 2));
}

void RaceModeDashboardView::packStateOfChargeReceived(double stateOfCharge)
{
    ui_.stateOfChargeCapacityWidget().setValue(stateOfCharge);
}

void RaceModeDashboardView::lowCellVoltageReceived(int lowVoltage)
{
    ui_.lowestCellVoltageLabel().setNum(lowVoltage);
}

void RaceModeDashboardView::averageCellVoltageReceived(int averageVoltage)
{
    ui_.avgCellVoltageLabel().setNum(averageVoltage);
}

void RaceModeDashboardView::highTemperatureReceived(int highTemp)
{
    ui_.maxCellTemperatureLabel().setText(QString::number(highTemp) + " " + TEMPERATURE_UNIT);
}

void RaceModeDashboardView::averageTemperatureReceived(int avgTemp)
{
    ui_.avgCellTemperatureLabel().setText(QString::number(avgTemp) + " " + TEMPERATURE_UNIT);
}

void RaceModeDashboardView::errorFlagsReceived(BatteryErrorFlags batteryErrorFlags)
{
    batteryFaultsList_.updateErrors(batteryErrorFlags);
    updateFaultLabel(ui_.batteryFaultsLabel(), batteryFaultsList_.getHighestActivePriorityLabel());
}
void RaceModeDashboardView::limitFlagsReceived(BatteryLimitFlags batteryLimitFlags)
{
    batteryFaultsList_.updateLimits(batteryLimitFlags);
    updateFaultLabel(ui_.batteryFaultsLabel(), batteryFaultsList_.getHighestActivePriorityLabel());
}
void RaceModeDashboardView::resetReceived(bool reset)
{
    if (reset)
    {
        ui_.motorResetButtonWidget().setStyleSheet("background: url(:/Resources/MotorResetButton.png)");
    }
    else
    {
        ui_.motorResetButtonWidget().setStyleSheet("");
    }
}
void RaceModeDashboardView::motorSetCurrentReceived(double setCurrent)
{
    ui_.setCurrentLabel().setText(QString::number(setCurrent, 'f', 3));
}
void RaceModeDashboardView::motorActualSpeedReceived(double actualSpeed)
{
    ui_.actualSpeedLabel().setText(QString::number(actualSpeed, 'f', 1));
}
void RaceModeDashboardView::motorBusVoltageReceived(double busVoltage)
{
    ui_.busVoltageLabel().setText(QString::number(busVoltage, 'f', 2));
    busVoltage_ = busVoltage;
    setMotorPower();
}
void RaceModeDashboardView::motorBusCurrentReceived(double busCurrent)
{
    ui_.busCurrentLabel().setText(QString::number(busCurrent, 'f', 2));
    busCurrent_ = busCurrent;
    setMotorPower();
}

void RaceModeDashboardView::setMotorPower()
{
    ui_.motorPowerLabel().setText(QString::number((busVoltage_ * busCurrent_), 'f', 2));
}

void RaceModeDashboardView::lowBeamsReceived(bool lowBeams)
{
    if (lowBeams)
    {
        ui_.lowHeadlightIndicatorWidget().setStyleSheet("border-image: url(:/Resources/LowHeadlightIndicator.png) 0 0 0 0 stretch stretch");
    }
    else
    {
        ui_.lowHeadlightIndicatorWidget().setStyleSheet("");
    }
}
void RaceModeDashboardView::highBeamsReceived(bool highBeams)
{
    if (highBeams)
    {
        ui_.highHeadlightIndicatorWidget().setStyleSheet("border-image: url(:/Resources/HighHeadlightIndicator.png) 0 0 0 0 stretch stretch");
    }
    else
    {
        ui_.highHeadlightIndicatorWidget().setStyleSheet("");
    }
}
void RaceModeDashboardView::leftSignalReceived(bool leftSignal)
{
    if (leftSignal)
    {
        ui_.leftTurnSignalWidget().setStyleSheet("border-image: url(:/Resources/TurnSignalLeft.png) 0 0 0 0 stretch stretch;");
    }
    else
    {
        ui_.leftTurnSignalWidget().setStyleSheet("");
    }
}
void RaceModeDashboardView::rightSignalReceived(bool rightSignal)
{
    if (rightSignal)
    {
        ui_.rightTurnSignalWidget().setStyleSheet("border-image: url(:/Resources/TurnSignalRight.png) 0 0 0 0 stretch stretch;");
    }
    else
    {
        ui_.rightTurnSignalWidget().setStyleSheet("");
    }
}
void RaceModeDashboardView::lightAliveReceived(bool)
{

}

void RaceModeDashboardView::mpptReceived(int i, Mppt mppt)
{
    if (i == 0)
    {
        mpptZeroPower_ = mppt.arrayCurrent() * mppt.arrayVoltage();
    }

    else if (i == 1)
    {
        mpptOnePower_ = mppt.arrayCurrent() * mppt.arrayVoltage();
    }

    else if (i == 2)
    {
        mpptTwoPower_ = mppt.arrayCurrent() * mppt.arrayVoltage();
    }
    else if (i == 3)
    {
        mpptThreePower_ = mppt.arrayCurrent() * mppt.arrayVoltage();
    }

    mpptPowerReceived(mpptZeroPower_ + mpptOnePower_ + mpptTwoPower_ + mpptThreePower_);
}

void RaceModeDashboardView::mpptPowerReceived(double mpptPower)
{
    ui_.powerInLabel().setText(QString::number(mpptPower, 'f', 1));
    ui_.powerOutLabel().setText(QString::number(qAbs(ui_.netPowerLabel().text().toDouble() - mpptPower), 'f', 1));
}

void RaceModeDashboardView::motorZeroErrorFlagsReceived(ErrorFlags flags)
{
    motorZeroFaultsList_.updateErrors(flags);
    updateFaultLabel(ui_.motorZeroFaultsLabel(), motorZeroFaultsList_.getHighestActivePriorityLabel());
}

void RaceModeDashboardView::motorZeroLimitFlagsReceived(LimitFlags flags)
{
    motorZeroFaultsList_.updateLimits(flags);
    updateFaultLabel(ui_.motorZeroFaultsLabel(), motorZeroFaultsList_.getHighestActivePriorityLabel());
}

void RaceModeDashboardView::motorOneErrorFlagsReceived(ErrorFlags flags)
{
    motorOneFaultsList_.updateErrors(flags);
    updateFaultLabel(ui_.motorOneFaultsLabel(), motorOneFaultsList_.getHighestActivePriorityLabel());
}

void RaceModeDashboardView::motorOneLimitFlagsReceived(LimitFlags flags)
{
    motorOneFaultsList_.updateLimits(flags);
    updateFaultLabel(ui_.motorOneFaultsLabel(), motorOneFaultsList_.getHighestActivePriorityLabel());
}
<|endoftext|>


<commit_before>#include "gtest/gtest.h"

#include <vector>
#include <cstdio>

#include "ZBE/core/events/EventStore.h"
#include "ZBE/core/events/generators/CollisionEventGenerator.h"
#include "ZBE/core/events/handlers/Actuator.h"
#include "ZBE/core/tools/containers/TicketedForwardList.h"
#include "ZBE/core/entities/adaptorentities/CollisionerEntity.h"
#include "ZBE/core/entities/adaptorentities/CollisionatorEntity.h"
#include "ZBE/core/entities/Entity.h"
#include "ZBE/core/entities/avatars/Collisioner.h"
#include "ZBE/core/entities/avatars/Collisionator.h"
#include "ZBE/core/entities/avatars/ReactObject.h"

namespace CollisionEventGeneratorTest {
class Robject;
class R { // Reactor mock
  public:
    ~R(){};
    virtual void act(Robject *) {};
};

class Coner : public zbe::CollisionerCommon<Coner, R> {
  public:
    Coner(zbe::CollisionObject<R> * co, std::forward_list<zbe::Actuator<Coner, R>* >* actuators, int id) : zbe::CollisionerCommon<Coner, R>(this, co, actuators),  id(id), vs(0){}
    ~Coner(){}
    int id;
  	int vs;
};

class Cator : public zbe::CollisionatorCommon<Cator, R> {
  public:
    Cator(zbe::CollisionObject<R> * co, std::forward_list<zbe::Actuator<Cator, R>* >* actuators, int id) : zbe::CollisionatorCommon<Cator, R>(this, co, actuators),  id(id), vs(0) {}
    ~Cator(){}
    int id;
  	int vs;
};

class Robject : public zbe::ReactObjectCommon<Robject, R> {
  public:
  	Robject(int id) : zbe::ReactObjectCommon<Robject, R>(this), id(id) {};
  	int id;
};

class ConerActuator: public zbe::Actuator<Coner, R> {
  public:
    void act(Robject * b) {
      collisioner->vs = b->id;
    }
};

class CatorActuator: public zbe::Actuator<Cator, R> {
  public:
    void act(Robject * b) {
      collisioner->vs = b->id;
    }
};

class DummyCollisionerEntity : public zbe::CollisionerEntity<R> {
public:
  DummyCollisionerEntity(const DummyCollisionerEntity&) = delete;
  void operator=(const DummyCollisionerEntity&) = delete;

  DummyCollisionerEntity(zbe::CollisionObject<R> * object, std::forward_list<zbe::Actuator<Coner,R>* >* actuators)
    : object(object), actuators(actuators), c(std::make_shared<Coner>(object, actuators, 42)), r(std::make_shared<Robject>(42)) {}

  std::shared_ptr<zbe::Collisioner<R> > getCollisioner() { return c;}
  std::shared_ptr<zbe::ReactObject<R> > getReactObject() { return r;}
  int getId(){return c->id;}
  int getVs(){return c->vs;}
  zbe::CollisionObject<R>* object;
  std::forward_list<zbe::Actuator<Coner,R>* >* actuators;
  std::shared_ptr<Coner> c;
  std::shared_ptr<Robject> r;
};

class DummyCollisionatorEntity : public zbe::CollisionatorEntity<R> {
public:
  DummyCollisionatorEntity(const DummyCollisionatorEntity&) = delete;
  void operator=(const DummyCollisionatorEntity&) = delete;

  DummyCollisionatorEntity(zbe::CollisionObject<R>* object, std::forward_list<zbe::Actuator<Cator,R>* >* actuators)
    : object(object), actuators(actuators), c(std::make_shared<Cator>(object, actuators, 37)), r(std::make_shared<Robject>(37)) {}

  std::shared_ptr<zbe::ReactObject<R> >   getReactObject()   { return r;}
  std::shared_ptr<zbe::Collisioner<R> >   getCollisioner() { return c;}
  std::shared_ptr<zbe::Collisionator<R> > getCollisionator() { return c;}
  int getId(){return c->id;}
  int getVs(){return c->vs;}
  zbe::CollisionObject<R>* object;
  std::forward_list<zbe::Actuator<Cator,R>* >* actuators;
  std::shared_ptr<Cator > c;
  std::shared_ptr<Robject> r;
};

TEST(CollisionEventGenerator, Generate) {
  zbe::TicketedForwardList<zbe::CollisionerEntity<R>*> cnl;
  zbe::TicketedForwardList<zbe::CollisionatorEntity<R>*> ctl;

  zbe::StaticAABB2D<R> sbox(zbe::AABB2D({{1.0,5.0},{6.0,10.0}}));
  zbe::ConstantMovingCircle<R> cc(zbe::Circle({{2.0,3.0},1.0}),zbe::Vector2D({3.0,4.0}));

  std::forward_list<zbe::Actuator<Coner,R>* > actconer;
  std::forward_list<zbe::Actuator<Cator,R>* > actcator;

  ConerActuator cna;
  CatorActuator cta;

  actconer.push_front(&cna);
  actcator.push_front(&cta);

  DummyCollisionerEntity dconer(&sbox, &actconer);
  DummyCollisionatorEntity dcator(&cc, &actcator);

  cnl.push_front(&dconer);
  ctl.push_front(&dcator);

  zbe::ListManager< zbe::TicketedForwardList<zbe::CollisionatorEntity<R>*> >& lmct = zbe::ListManager< zbe::TicketedForwardList<zbe::CollisionatorEntity<R>*> >::getInstance();
  zbe::ListManager< zbe::TicketedForwardList<zbe::CollisionerEntity<R>*> >& lmcn = zbe::ListManager< zbe::TicketedForwardList<zbe::CollisionerEntity<R>*> >::getInstance();

  lmcn.insert(1, &cnl);
  lmct.insert(2, &ctl);

  dcator.getCollisionator()->addToCollisionablesLists(1);

  zbe::CollisionEventGenerator<R> ceg(2, 1);

  zbe::EventStore &es = zbe::EventStore::getInstance();
  ASSERT_TRUE(es.getEvents().empty()) << "List must be empty.";
  ceg.generate(0, 2 * zbe::VELOCITYTOTIME);
  es.manageCurrent();

  EXPECT_EQ(42, dconer.getId()) << "Coner id must be 42";
  EXPECT_EQ(37, dconer.getVs()) << "Coner is must be 37";
  EXPECT_EQ(37, dcator.getId()) << "Cator id must be 37";
  EXPECT_EQ(42, dcator.getVs()) << "Cator is must be 42";
}
} //namespace CollisionEventGeneratorTest
<commit_msg>Shameful minor fix.<commit_after>#include "gtest/gtest.h"

#include <vector>
#include <cstdio>

#include "ZBE/core/events/EventStore.h"
#include "ZBE/core/events/generators/CollisionEventGenerator.h"
#include "ZBE/core/events/handlers/Actuator.h"
#include "ZBE/core/tools/containers/TicketedForwardList.h"
#include "ZBE/core/entities/adaptorentities/CollisionerEntity.h"
#include "ZBE/core/entities/adaptorentities/CollisionatorEntity.h"
#include "ZBE/core/entities/Entity.h"
#include "ZBE/core/entities/avatars/Collisioner.h"
#include "ZBE/core/entities/avatars/Collisionator.h"
#include "ZBE/core/entities/avatars/ReactObject.h"

namespace CollisionEventGeneratorTest {
class Robject;
class R { // Reactor mock
  public:
    virtual ~R(){};
    virtual void act(Robject *) {};
};

class Coner : public zbe::CollisionerCommon<Coner, R> {
  public:
    Coner(zbe::CollisionObject<R> * co, std::forward_list<zbe::Actuator<Coner, R>* >* actuators, int id) : zbe::CollisionerCommon<Coner, R>(this, co, actuators),  id(id), vs(0){}
    ~Coner(){}
    int id;
  	int vs;
};

class Cator : public zbe::CollisionatorCommon<Cator, R> {
  public:
    Cator(zbe::CollisionObject<R> * co, std::forward_list<zbe::Actuator<Cator, R>* >* actuators, int id) : zbe::CollisionatorCommon<Cator, R>(this, co, actuators),  id(id), vs(0) {}
    ~Cator(){}
    int id;
  	int vs;
};

class Robject : public zbe::ReactObjectCommon<Robject, R> {
  public:
  	Robject(int id) : zbe::ReactObjectCommon<Robject, R>(this), id(id) {};
  	int id;
};

class ConerActuator: public zbe::Actuator<Coner, R> {
  public:
    void act(Robject * b) {
      collisioner->vs = b->id;
    }
};

class CatorActuator: public zbe::Actuator<Cator, R> {
  public:
    void act(Robject * b) {
      collisioner->vs = b->id;
    }
};

class DummyCollisionerEntity : public zbe::CollisionerEntity<R> {
public:
  DummyCollisionerEntity(const DummyCollisionerEntity&) = delete;
  void operator=(const DummyCollisionerEntity&) = delete;

  DummyCollisionerEntity(zbe::CollisionObject<R> * object, std::forward_list<zbe::Actuator<Coner,R>* >* actuators)
    : object(object), actuators(actuators), c(std::make_shared<Coner>(object, actuators, 42)), r(std::make_shared<Robject>(42)) {}

  std::shared_ptr<zbe::Collisioner<R> > getCollisioner() { return c;}
  std::shared_ptr<zbe::ReactObject<R> > getReactObject() { return r;}
  int getId(){return c->id;}
  int getVs(){return c->vs;}
  zbe::CollisionObject<R>* object;
  std::forward_list<zbe::Actuator<Coner,R>* >* actuators;
  std::shared_ptr<Coner> c;
  std::shared_ptr<Robject> r;
};

class DummyCollisionatorEntity : public zbe::CollisionatorEntity<R> {
public:
  DummyCollisionatorEntity(const DummyCollisionatorEntity&) = delete;
  void operator=(const DummyCollisionatorEntity&) = delete;

  DummyCollisionatorEntity(zbe::CollisionObject<R>* object, std::forward_list<zbe::Actuator<Cator,R>* >* actuators)
    : object(object), actuators(actuators), c(std::make_shared<Cator>(object, actuators, 37)), r(std::make_shared<Robject>(37)) {}

  std::shared_ptr<zbe::ReactObject<R> >   getReactObject()   { return r;}
  std::shared_ptr<zbe::Collisioner<R> >   getCollisioner() { return c;}
  std::shared_ptr<zbe::Collisionator<R> > getCollisionator() { return c;}
  int getId(){return c->id;}
  int getVs(){return c->vs;}
  zbe::CollisionObject<R>* object;
  std::forward_list<zbe::Actuator<Cator,R>* >* actuators;
  std::shared_ptr<Cator > c;
  std::shared_ptr<Robject> r;
};

TEST(CollisionEventGenerator, Generate) {
  zbe::TicketedForwardList<zbe::CollisionerEntity<R>*> cnl;
  zbe::TicketedForwardList<zbe::CollisionatorEntity<R>*> ctl;

  zbe::StaticAABB2D<R> sbox(zbe::AABB2D({{1.0,5.0},{6.0,10.0}}));
  zbe::ConstantMovingCircle<R> cc(zbe::Circle({{2.0,3.0},1.0}),zbe::Vector2D({3.0,4.0}));

  std::forward_list<zbe::Actuator<Coner,R>* > actconer;
  std::forward_list<zbe::Actuator<Cator,R>* > actcator;

  ConerActuator cna;
  CatorActuator cta;

  actconer.push_front(&cna);
  actcator.push_front(&cta);

  DummyCollisionerEntity dconer(&sbox, &actconer);
  DummyCollisionatorEntity dcator(&cc, &actcator);

  cnl.push_front(&dconer);
  ctl.push_front(&dcator);

  zbe::ListManager< zbe::TicketedForwardList<zbe::CollisionatorEntity<R>*> >& lmct = zbe::ListManager< zbe::TicketedForwardList<zbe::CollisionatorEntity<R>*> >::getInstance();
  zbe::ListManager< zbe::TicketedForwardList<zbe::CollisionerEntity<R>*> >& lmcn = zbe::ListManager< zbe::TicketedForwardList<zbe::CollisionerEntity<R>*> >::getInstance();

  lmcn.insert(1, &cnl);
  lmct.insert(2, &ctl);

  dcator.getCollisionator()->addToCollisionablesLists(1);

  zbe::CollisionEventGenerator<R> ceg(2, 1);

  zbe::EventStore &es = zbe::EventStore::getInstance();
  ASSERT_TRUE(es.getEvents().empty()) << "List must be empty.";
  ceg.generate(0, 2 * zbe::VELOCITYTOTIME);
  es.manageCurrent();

  EXPECT_EQ(42, dconer.getId()) << "Coner id must be 42";
  EXPECT_EQ(37, dconer.getVs()) << "Coner is must be 37";
  EXPECT_EQ(37, dcator.getId()) << "Cator id must be 37";
  EXPECT_EQ(42, dcator.getVs()) << "Cator is must be 42";
}
} //namespace CollisionEventGeneratorTest
<|endoftext|>