/* discriminative_trainer.cc
   Jeremy Barnes, 9 November 2009
   Copyright (c) 2009 Jeremy Barnes.  All rights reserved.

   Discriminative training for neural network architectures via backprop.
*/

#include "discriminative_trainer.h"
#include "jml/utils/worker_task.h"
#include "jml/utils/guard.h"
#include <boost/progress.hpp>
#include <boost/tuple/tuple.hpp>
#include "jml/arch/threads.h"
#include <boost/bind.hpp>
#include "jml/arch/timers.h"
#include "jml/stats/auc.h"

using namespace std;

namespace ML {

/*****************************************************************************/
/* DISCRIMINATIVE_TRAINER                                                    */
/*****************************************************************************/

pair<double, double>
Discriminative_Trainer::
train_example(const distribution<float> & data,
              Label label,
              Parameters_Copy<double> & updates,
              const Output_Encoder & encoder,
              float weight) const
{
    distribution<float> label_dist = encoder.target(label);
    return train_example(&data[0], &label_dist[0], updates, weight);
}

pair<double, double>
Discriminative_Trainer::
train_example(const float * data,
              Label label,
              Parameters_Copy<double> & updates,
              const Output_Encoder & encoder,
              float weight) const
{
    distribution<float> label_dist = encoder.target(label);
    return train_example(data, &label_dist[0], updates, weight);
}

pair<double, double>
Discriminative_Trainer::
train_example(const distribution<float> & data,
              const distribution<float> & label,
              Parameters_Copy<double> & updates,
              float weight) const
{
    if (data.size() != layer->inputs())
        throw Exception("data had wrong length");
    if (label.size() != layer->outputs())
        throw Exception("layer had wrong length");

    return train_example(&data[0], &label[0], updates, weight);
}

pair<double, double>
Discriminative_Trainer::
train_example(const float * data,
              const float * label,
              Parameters_Copy<double> & updates,
              float weight) const
{
    /* fprop */

    size_t temp_space_required
        = layer->fprop_temporary_space_required();

    float temp_space[temp_space_required];

    int no = layer->outputs();

    distribution<float> outputs(no);
    layer->fprop(&data[0], temp_space, temp_space_required, &outputs[0]);

    /* error */

    distribution<float> labelv(label, label + no);

    distribution<float> errors = (labelv - outputs);
    double error = errors.dotprod(errors);

    if (weight != 0.0) {
        // TODO: get the loss function to do this...
        distribution<float> derrors = -2.0 * errors;

        /* bprop */
        layer->bprop(&data[0],
                     &outputs[0],
                     temp_space, temp_space_required,
                     &derrors[0],
                     0 /* don't calculate input errors */,
                     updates,
                     weight);
    }

    if (false) {
        distribution<float> in(data, data + layer->inputs());
        cerr << "in " << in << endl;
        cerr << "outputs " << outputs << endl;
        cerr << "labelv " << labelv << endl;
        cerr << "errors " << errors << endl;

        Parameters_Copy<float> params = layer->parameters();

        cerr << "params " << params.values << endl;
        cerr << "updates " << updates.values << endl;
    }

    return make_pair(sqrt(error), outputs[0]);
}

namespace {

struct Train_Examples_Job {

    const Discriminative_Trainer & trainer;
    const std::vector<const float *> & data;
    const std::vector<Label> & labels;
    const std::vector<float> & weights;
    const Output_Encoder & output_encoder;
    Thread_Context & thread_context;
    const vector<int> & examples;
    int first;
    int last;
    Parameters_Copy<double> & updates;
    vector<float> & outputs;
    double & total_rmse;
    Lock & updates_lock;
    boost::progress_display * progress;
    int verbosity;

    Train_Examples_Job(const Discriminative_Trainer & trainer,
                       const std::vector<const float *> & data,
                       const std::vector<Label> & labels,
                       const std::vector<float> & weights,
                       const Output_Encoder & output_encoder,
                       Thread_Context & thread_context,
                       const vector<int> & examples,
                       int first, int last,
                       Parameters_Copy<double> & updates,
                       vector<float> & outputs,
                       double & total_rmse,
                       Lock & updates_lock,
                       boost::progress_display * progress,
                       int verbosity)
        : trainer(trainer), data(data), labels(labels), weights(weights),
          output_encoder(output_encoder),
          thread_context(thread_context), examples(examples),
          first(first), last(last), updates(updates), outputs(outputs),
          total_rmse(total_rmse), updates_lock(updates_lock),
          progress(progress), verbosity(verbosity)
    {
    }

    void operator () ()
    {
        Parameters_Copy<double> local_updates(*trainer.layer);
        local_updates.fill(0.0);

        double total_rmse_local = 0.0;

        //cerr << "training from " << first << " to " << last << endl;

        for (unsigned ix = first; ix < last;  ++ix) {
            int x = examples[ix];

            double rmse_contribution;
            double output;

            float weight = 1.0;
            if (weights.size())
                weight = weights.at(x);

            //cerr << "x " << x << " weight " << weight << endl;

            boost::tie(rmse_contribution, output)
                = trainer.train_example(data[x],
                                        labels[x],
                                        local_updates,
                                        output_encoder,
                                        weight);

            outputs[ix] = output;
            total_rmse_local += rmse_contribution;
        }

        Guard guard(updates_lock);
        total_rmse += total_rmse_local;
        updates.values += local_updates.values;
        if (progress) progress += (last - first);
    }
};

} // file scope

std::pair<double, double>
Discriminative_Trainer::
train_iter(const std::vector<distribution<float> > & data,
           const std::vector<Label> & labels,
           const std::vector<float> & weights,
           const Output_Encoder & output_encoder,
           Thread_Context & thread_context,
           int minibatch_size,
           float learning_rate,
           int verbosity,
           float sample_proportion,
           bool randomize_order) const
{
    vector<const float *> data2(data.size());
    for (unsigned i = 0;  i < data.size();  ++i)
        data2[i] = &data[i][0];

    return train_iter(data2, labels, weights,
                      output_encoder, thread_context, minibatch_size,
                      learning_rate, verbosity, sample_proportion,
                      randomize_order);
}

std::pair<double, double>
Discriminative_Trainer::
train_iter(const std::vector<const float *> & data,
           const std::vector<Label> & labels,
           const std::vector<float> & weights,
           const Output_Encoder & output_encoder,
           Thread_Context & thread_context,
           int minibatch_size, float learning_rate,
           int verbosity,
           float sample_proportion,
           bool randomize_order) const
{
    Worker_Task & worker = thread_context.worker();

    int nx = data.size();

    int microbatch_size = std::max(minibatch_size / (num_threads() * 4), 1);

    Lock update_lock;

    vector<int> examples;
    for (unsigned x = 0;  x < nx;  ++x) {
        // Randomly exclude some samples
        if (thread_context.random01() >= sample_proportion)
            continue;
        examples.push_back(x);
    }

    if (randomize_order) {
        Thread_Context::RNG_Type rng = thread_context.rng();
        std::random_shuffle(examples.begin(), examples.end(), rng);
    }

    int nx2 = examples.size();

    double total_mse = 0.0;
    distribution<float> outputs;
    outputs.resize(nx2);

    std::unique_ptr<boost::progress_display> progress;
    if (verbosity >= 3) progress.reset(new boost::progress_display(nx2, cerr));

    for (unsigned x = 0;  x < nx2;  x += minibatch_size) {

        Parameters_Copy<double> updates(*layer);
        updates.fill(0.0);

        // Now, submit it as jobs to the worker task to be done
        // multithreaded
        int group;
        {
            int parent = -1;  // no parent group
            group = worker.get_group(NO_JOB, "dump user results task",
                                     parent);

            // Make sure the group gets unlocked once we've populated
            // everything
            Call_Guard guard(boost::bind(&Worker_Task::unlock_group,
                                         boost::ref(worker),
                                         group));

            for (unsigned x2 = x;  x2 < nx2 && x2 < x + minibatch_size;
                 x2 += microbatch_size) {

                Train_Examples_Job
                    job(*this,
                        data,
                        labels,
                        weights,
                        output_encoder,
                        thread_context,
                        examples,
                        x2,
                        min<int>(nx2,
                                 min(x + minibatch_size,
                                     x2 + microbatch_size)),
                        updates,
                        outputs,
                        total_mse,
                        update_lock,
                        progress.get(),
                        verbosity);

                // Send it to a thread to be processed
                worker.add(job, "backprop job", group);
            }
        }

        worker.run_until_finished(group);

        //cerr << "finished minibatch: updates = " << updates.values
        //     << " learning_rate = " << learning_rate << endl;

        //cerr << "applying minibatch updates" << endl;

        //cerr << "updates.values = " << updates.values << endl;
        //cerr << "learning_rate = " << learning_rate << endl;

        layer->parameters().update(updates, -learning_rate);

        //cerr << "final value = "
        //     << Parameters_Copy<double>(layer->parameters()).values
        //     << endl;
    }

    // TODO: calculate AUC score
    distribution<float> test_labels;
    for (unsigned i = 0;  i < nx2;  ++i)
        test_labels.push_back(labels[examples[i]]);

    float neg = 0.0, pos = 1.0;

    double auc = calc_auc(outputs, test_labels, neg, pos);

    return make_pair(sqrt(total_mse / nx2), auc);
}

std::pair<double, double>
Discriminative_Trainer::
train(const std::vector<distribution<float> > & training_data,
      const std::vector<Label> & training_labels,
      const std::vector<float> & training_weights,
      const std::vector<distribution<float> > & testing_data,
      const std::vector<Label> & testing_labels,
      const std::vector<float> & testing_weights,
      const Configuration & config,
      ML::Thread_Context & thread_context) const
{
    double learning_rate = 0.75;
    int minibatch_size = 512;
    int niter = 50;
    int verbosity = 2;

    bool randomize_order = true;
    float sample_proportion = 0.8;
    int test_every = 1;

    Output_Encoder output_encoder;
    output_encoder.configure(config, *layer);

    config.get(learning_rate, "learning_rate");
    config.get(minibatch_size, "minibatch_size");
    config.get(niter, "niter");
    config.get(verbosity, "verbosity");
    config.get(randomize_order, "randomize_order");
    config.get(sample_proportion, "sample_proportion");
    config.get(test_every, "test_every");

    int nx = training_data.size();

    if (training_data.size() != training_labels.size())
        throw Exception("label and example sizes don't match");

    int nxt = testing_data.size();

    if (nx == 0)
        throw Exception("can't train on no data");

    // Learning rate is per-example
    learning_rate /= nx;

    // Compensate for the example proportion
    learning_rate /= sample_proportion;

    if (verbosity == 2)
        cerr << "iter  ---- train ----  ---- test -----\n"
             << "         rmse     auc     rmse     auc\n";

    for (unsigned iter = 0;  iter < niter;  ++iter) {
        if (verbosity >= 3)
            cerr << "iter " << iter << " training on " << nx << " examples"
                 << endl;
        else if (verbosity >= 2)
            cerr << format("%4d", iter) << flush;
        Timer timer;

        double train_error_rmse, train_error_auc;
        boost::tie(train_error_rmse, train_error_auc)
            = train_iter(training_data, training_labels, training_weights,
                         output_encoder, thread_context,
                         minibatch_size, learning_rate,
                         verbosity, sample_proportion,
                         randomize_order);

        if (verbosity >= 3) {
            cerr << "error of iteration: rmse " << train_error_rmse
                 << " noisy " << train_error_auc << endl;
            if (verbosity >= 3) cerr << timer.elapsed() << endl;
        }
        else if (verbosity == 2)
            cerr << format("  %7.5f %7.5f",
                           train_error_rmse, train_error_auc)
                 << flush;

        if (iter % test_every == (test_every - 1)
            || iter == niter - 1) {
            timer.restart();
            double test_error_rmse = 0.0, test_error_auc = 0.0;

            if (verbosity >= 3)
                cerr << "testing on " << nxt << " examples"
                     << endl;

            boost::tie(test_error_rmse, test_error_auc)
                = test(testing_data, testing_labels, testing_weights,
                       output_encoder, thread_context, verbosity);

            if (verbosity >= 3) {
                cerr << "testing error of iteration: rmse "
                     << test_error_rmse << " auc " << test_error_auc
                     << endl;
                cerr << timer.elapsed() << endl;
            }
            else if (verbosity == 2)
                cerr << format("  %7.5f %7.5f",
                               test_error_rmse, test_error_auc);
        }

        if (verbosity == 2) cerr << endl;
    }

    // TODO: return something
    return make_pair(0.0, 0.0);
}

namespace {

struct Test_Examples_Job {

    const Discriminative_Trainer & trainer;
    const vector<const float *> & data;
    const vector<Label> & labels;
    const vector<float> & weights;
    const Output_Encoder & output_encoder;
    int first;
    int last;
    const Thread_Context & context;
    Lock & update_lock;
    double & error_rmse;
    vector<float> & outputs;
    boost::progress_display * progress;
    int verbosity;

    Test_Examples_Job(const Discriminative_Trainer & trainer,
                      const vector<const float *> & data,
                      const vector<Label> & labels,
                      const vector<float> & weights,
                      const Output_Encoder & output_encoder,
                      int first, int last,
                      const Thread_Context & context,
                      Lock & update_lock,
                      double & error_rmse,
                      vector<float> & outputs,
                      boost::progress_display * progress,
                      int verbosity)
        : trainer(trainer), data(data), labels(labels), weights(weights),
          output_encoder(output_encoder),
          first(first), last(last),
          context(context),
          update_lock(update_lock),
          error_rmse(error_rmse), outputs(outputs),
          progress(progress), verbosity(verbosity)
    {
    }

    void operator () ()
    {
        double local_error_rmse = 0.0;

        for (unsigned x = first;  x < last;  ++x) {
            distribution<float> output(trainer.layer->outputs());
            trainer.layer->apply(data[x], &output[0]);

            outputs[x] = output[0];
            local_error_rmse += pow(labels[x] - output[0], 2);
        }

        Guard guard(update_lock);
        error_rmse += local_error_rmse;
        if (progress && verbosity >= 3) (*progress) += (last - first);
    }
};

} // file scope

pair<double, double>
Discriminative_Trainer::
test(const std::vector<distribution<float> > & data,
     const std::vector<Label> & labels,
     const std::vector<float> & weights,
     const Output_Encoder & output_encoder,
     ML::Thread_Context & thread_context,
     int verbosity) const
{
    vector<const float *> data2(data.size());
    for (unsigned i = 0;  i < data.size();  ++i)
        data2[i] = &data[i][0];
    return test(data2, labels, weights, output_encoder,
                thread_context, verbosity);
}

pair<double, double>
Discriminative_Trainer::
test(const std::vector<const float *> & data,
     const std::vector<Label> & labels,
     const std::vector<float> & weights,
     const Output_Encoder & output_encoder,
     ML::Thread_Context & thread_context,
     int verbosity) const
{
    Lock update_lock;
    double mse_total = 0.0;

    int nx = data.size();

    std::unique_ptr<boost::progress_display> progress;
    if (verbosity >= 3) progress.reset(new boost::progress_display(nx, cerr));

    Worker_Task & worker = thread_context.worker();

    distribution<float> outputs;
    outputs.resize(nx);

    // Now, submit it as jobs to the worker task to be done
    // multithreaded
    int group;
    {
        int parent = -1;  // no parent group
        group = worker.get_group(NO_JOB, "dump user results task",
                                 parent);

        // Make sure the group gets unlocked once we've populated
        // everything
        Call_Guard guard(boost::bind(&Worker_Task::unlock_group,
                                     boost::ref(worker),
                                     group));

        // 20 jobs per CPU
        int batch_size = nx / (num_threads() * 20);

        for (unsigned x = 0; x < nx;  x += batch_size) {

            Test_Examples_Job job(*this, data, labels, weights,
                                  output_encoder,
                                  x, min<int>(x + batch_size, nx),
                                  thread_context,
                                  update_lock,
                                  mse_total, outputs,
                                  progress.get(),
                                  verbosity);

            // Send it to a thread to be processed
            worker.add(job, "test discrim job", group);
        }
    }

    worker.run_until_finished(group);

    return make_pair(sqrt(mse_total / nx),
                     output_encoder.calc_auc(outputs, labels));
}

} // namespace ML


#include "ros/ros.h"
#include <gtest/gtest.h>
#include <Eigen/Dense>
#include <eigen_conversions/eigen_msg.h>
#include <vector>
#include <boost/array.hpp>

#include "vortex_msgs/PropulsionCommand.h"

class ControllerTest : public ::testing::Test
{
public:
  ControllerTest()
  {
    cmdPub = nh.advertise<vortex_msgs::PropulsionCommand>("propulsion_command", 10);
    sub = nh.subscribe("rov_forces", 10, &ControllerTest::Callback, this);
    message_received = false;
  }

  void SetUp()
  {
    while (!IsNodeReady())
      ros::spinOnce();
  }

  void PublishCommand(boost::array<double, 6> motion, std::vector<uint8_t> mode)
  {
    vortex_msgs::PropulsionCommand msg;
    msg.motion = motion;
    msg.control_mode = mode;
    cmdPub.publish(msg);
  }

  void ExpectTauNear(std::vector<double> arr)
  {
    for (int i = 0; i < tau.size(); ++i)
      EXPECT_NEAR(tau[i], arr[i], MAX_ERROR);
  }

  void WaitForMessage()
  {
    while (!message_received)
      ros::spinOnce();
  }

  Eigen::Matrix<double, 6, 1> tau;
  const double MAX_ERROR = 0.0001;

private:
  ros::NodeHandle nh;
  ros::Publisher cmdPub;
  ros::Subscriber sub;

  bool message_received;

  void Callback(const geometry_msgs::Wrench& msg)
  {
    tf::wrenchMsgToEigen(msg, tau);
    message_received = true;
  }

  bool IsNodeReady()
  {
    return ((cmdPub.getNumSubscribers() > 0) && (sub.getNumPublishers() > 0));
  }
};

TEST_F(ControllerTest, CheckResponsiveness)
{
  WaitForMessage();
}

TEST_F(ControllerTest, OpenLoop)
{
  PublishCommand({1, 0, 0, 0, 0, 0}, {1, 0, 0, 0});  // NOLINT(whitespace/braces)
  ros::Duration(0.5).sleep();
  WaitForMessage();

  ExpectTauNear({48.895, 0, 0, 0, 0, 0});  // NOLINT(whitespace/braces)
}

int main(int argc, char **argv)
{
  testing::InitGoogleTest(&argc, argv);
  ros::init(argc, argv, "controller_test");

  int ret = RUN_ALL_TESTS();
  ros::shutdown();
  return ret;
}


#pragma once

#include <deque>
#include <unordered_map>
#include <vector>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/index/rtree.hpp>

#include "../planning-utils/geom/Coord.hpp"
#include "../planning-utils/geom/Rect.hpp"
#include "../planning-utils/visibility/EndPoint.hpp"
#include "../planning-utils/visibility/Segment.hpp"
#include "Halton.hpp"
#include "RrtNode.hpp"

typedef boost::geometry::model::box<point> box;
typedef std::pair<point, std::shared_ptr<RrtNode>> RtreeValue;
typedef boost::geometry::index::rtree<RtreeValue, boost::geometry::index::rstar<16>> Rtree;

class Planner {
   protected:
	int width;
	int height;
	double mapArea;
	double obstacleArea;
	std::shared_ptr<std::vector<std::vector<bool>>> obstacleHash;
	double maxTravel = 1.5;
	std::unordered_map<Coord, double> unseenAreaMap;

	HaltonSampler haltonX;
	HaltonSampler haltonY;

	Rtree rtree;

	std::vector<std::shared_ptr<Segment>> segments;
	std::vector<std::shared_ptr<EndPoint>> endpoints;

	Coord randomOpenAreaPoint();
	double getViewArea(Coord point);
	double getUnseenArea(Coord point);
	double getEdgeUnseenArea(Coord start, Coord end, double dist);
	double getCost(std::shared_ptr<RrtNode> start, std::shared_ptr<RrtNode> end);
	double getCost(Coord start, Coord end);

	bool lineIntersectsObstacle(Coord p1, Coord p2);

	void refreshBestPath();

	void getNeighbors(Coord &center, double radius, std::vector<RtreeValue> &results);

   public:
	std::shared_ptr<RrtNode> root;
	std::shared_ptr<RrtNode> endNode;
	std::shared_ptr<std::vector<std::shared_ptr<Rect>>> obstacleRects;
	std::deque<Coord> bestPath;
	bool usePseudoRandom;
	std::string name = "unset";

	const double distanceK = 1.0;
	double unseenAreaK = 100.0;

	Planner(std::shared_ptr<std::vector<std::vector<bool>>> obstacleHash, std::shared_ptr<std::vector<std::shared_ptr<Rect>>> obstacleRects, int width, int height,
	        bool usePseudoRandom);
	virtual ~Planner();
	virtual void moveStart(double dx, double dy);
	virtual void replan(Coord &newEndpoint);
	void randomReplan();
	void followPath();
	double calculatePathCost();

	std::deque<Coord> getBestPath() { return this->bestPath; }

	std::vector<Coord> calculateVisibilityPolygon(Coord origin);
};


#include <string>
#include <string.h>
#include <stdlib.h>
#include <iostream>
#include <cstdio>

#include <Eigen/Core>
#include <pcl/common/transforms.h>
#include <pcl/common/time.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/image_viewer.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/dinast_grabber.h>

#define FPS_CALC(_WHAT_)                                                       \
    do {                                                                       \
        static unsigned count = 0;                                             \
        static double last = pcl::getTime();                                   \
        double now = pcl::getTime();                                           \
        ++count;                                                               \
        if (now - last >= 1.0) {                                               \
            std::cout << "Average framerate(" << _WHAT_                        \
                      << "): " << double(count) / double(now - last) << " Hz"  \
                      << std::endl;                                            \
            count = 0;                                                         \
            last = now;                                                        \
        }                                                                      \
    } while (false)

void savePGM(const unsigned char *image, const std::string &filename) {
    FILE *file = fopen(filename.c_str(), "w");
    if (!file) {
        std::cerr << "Unable to open file '" << filename << "' for writing."
                  << std::endl;
        return;
    }

    // PGM header
    fprintf(file, "P2\n%d %d\n255\n", IMAGE_WIDTH, IMAGE_HEIGHT);

    // Write data as ASCII
    for (int i = 0; i < IMAGE_HEIGHT; ++i) {
        for (int j = 0; j < IMAGE_WIDTH; ++j) {
            fprintf(file, "%3d ", (int)*image++);
        }
        fprintf(file, "\n");
    }

    fclose(file);
}

void keyboardEventOccurred(const pcl::visualization::KeyboardEvent &event,
                           void *data) {
    static int NUM_IMAGES = 0;
    if (event.getKeySym() == "s" && event.keyDown()) {
        char filename[16];
        snprintf(filename, sizeof(filename), "image%.2d.pgm", NUM_IMAGES++);
        unsigned char *image = reinterpret_cast<unsigned char *>(data);
        savePGM(image, filename);
        printf("Wrote %s\n", filename);
    }
}

void convertImageToCloud(const unsigned char *image,
                         pcl::PointCloud<pcl::PointXYZI> &cloud) {
    cloud.points.resize(IMAGE_WIDTH * IMAGE_HEIGHT);
    cloud.width = IMAGE_WIDTH;
    cloud.height = IMAGE_HEIGHT;
    cloud.is_dense = false;

    int depth_idx = 0;
    int pxl[9];

    for (int x = 0; x < cloud.width; ++x) {
        for (int y = 0; y < cloud.height; ++y, ++depth_idx) {
            float xc = (float)(x - 160);
            float yc = (float)(y - 120);
            double r1 = sqrt(xc * xc + yc * yc);
            double r2 = r1 * r1;
            double r3 = r1 * r2;
            double A = -2e-5 * r3 + 0.004 * r2 + 0.1719 * r1 + 350.03;
            double B = -2e-9 * r3 + 3e-7 * r2 - 1e-5 * r1 - 0.01;

            // Low pass filtering
            /// @todo Try a bilateral filter to avoid blurring over depth
            /// boundaries
            int measure = 0;
            if ((y > 0) && (y < (IMAGE_HEIGHT - 1)) && (x > 0) &&
                (x < (IMAGE_WIDTH - 1))) {
                int ipx = x + IMAGE_WIDTH * y;
#if 1
                pxl[0] = image[ipx];
                pxl[1] = image[ipx - 1];
                pxl[2] = image[ipx + 1];
                pxl[3] = image[ipx - IMAGE_WIDTH];
                pxl[4] = image[ipx - IMAGE_WIDTH - 1];
                pxl[5] = image[ipx - IMAGE_WIDTH + 1];
                pxl[6] = image[ipx + IMAGE_WIDTH];
                pxl[7] = image[ipx + IMAGE_WIDTH - 1];
                pxl[8] = image[ipx + IMAGE_WIDTH + 1];

                for (int ii = 0; ii < 9; ii++)
                    measure += pxl[ii];
                measure /= 9;
#else
                // No blurring
                measure = image[ipx];
#endif
            }
            if (measure > 255)
                measure = 255; // saturation for display

            unsigned char pixel = measure; // image[depth_idx];
            if (pixel < 1) {
                cloud.points[depth_idx].x =
                    std::numeric_limits<float>::quiet_NaN();
                cloud.points[depth_idx].y =
                    std::numeric_limits<float>::quiet_NaN();
                cloud.points[depth_idx].z =
                    std::numeric_limits<float>::quiet_NaN();
                cloud.points[depth_idx].intensity = pixel;
                continue;
            }

            if (pixel > A)
                pixel = A;

            float dy = y * 0.1;
            double dist = (log((double)pixel / A) / B - dy) *
                          (7E-07 * r3 - 0.0001 * r2 + 0.004 * r1 + 0.9985) *
                          1.5;
            double dist_2d = r1;

            static const double dist_max_2d = 1 / 160.0; /// @todo Why not 200?
            // static const double dist_max_2d = 1 / 200.0;
            static const double FOV = 64.0 * M_PI / 180.0; // diagonal FOV?

            double theta_colati = FOV * r1 * dist_max_2d;
            double c_theta = cos(theta_colati);
            double s_theta = sin(theta_colati);
            double c_ksai = ((double)(x - 160.)) / r1;
            double s_ksai = ((double)(y - 120.)) / r1;

            cloud.points[depth_idx].x =
                (dist * s_theta * c_ksai) / 500.0 + 0.5; // cartesian x
            cloud.points[depth_idx].y =
                (dist * s_theta * s_ksai) / 500.0 + 0.5;  // cartesian y
            cloud.points[depth_idx].z = (dist * c_theta); // cartesian z
            /// @todo This looks weird, can it cause artifacts?
            if (cloud.points[depth_idx].z < 0.01)
#if 1
                cloud.points[depth_idx].z = 0.01;
#else
                cloud.points[depth_idx].x =
                    std::numeric_limits<float>::quiet_NaN();
            cloud.points[depth_idx].y = std::numeric_limits<float>::quiet_NaN();
            cloud.points[depth_idx].z = std::numeric_limits<float>::quiet_NaN();
            cloud.points[depth_idx].intensity = pixel;
            continue;
#endif

            cloud.points[depth_idx].z /= 500.0;
            cloud.points[depth_idx].intensity = pixel;
        }
    }
}

/* --[ */

int main(int argc, char **argv) {

    pcl::DinastGrabber grabber;

    grabber.findDevice(1);

    grabber.openDevice();

    std::cerr << "Device version/revision number: "
              << grabber.getDeviceVersion() << std::endl;

    grabber.start();

    pcl::visualization::ImageViewer vis_img("Dinast Image Viewer");
    pcl::visualization::PCLVisualizer vis_cld(argc, argv,
                                              "Dinast Cloud Viewer");

    unsigned char *img1 = (unsigned char *)malloc(IMAGE_SIZE);
    unsigned char *img2 = (unsigned char *)malloc(IMAGE_SIZE);

    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(
        new pcl::PointCloud<pcl::PointXYZI>);

    while (true) {
        if (grabber.readImage(img1, img2) == 0)
            continue;

        convertImageToCloud(img1, *cloud);

        FPS_CALC("grabber + visualization");
        vis_img.showMonoImage(img1, IMAGE_WIDTH, IMAGE_HEIGHT);

        pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI>
            handler(cloud, "intensity");
        if (!vis_cld.updatePointCloud(cloud, handler, "DinastCloud")) {
            vis_cld.addPointCloud(cloud, handler, "DinastCloud");
            vis_cld.resetCameraViewpoint("DinastCloud");
        }

        vis_img.spinOnce();
        vis_cld.spinOnce();
    }

    grabber.stop();
    grabber.closeDevice();
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// This file is manually converted from PROJ4

// Copyright (c) 2008-2012 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Geometry Library by
//     Barend Gehrels (Geodan, Amsterdam)
//     Adam Wulkiewicz

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#ifndef BOOST_GEOMETRY_PROJECTIONS_IMPL_PJ_ELL_SET_HPP
#define BOOST_GEOMETRY_PROJECTIONS_IMPL_PJ_ELL_SET_HPP

#include <string>
#include <vector>

#include <boost/geometry/formulas/eccentricity_sqr.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/srs/projections/dpar.hpp>
#include <boost/geometry/srs/projections/impl/pj_datum_set.hpp>
#include <boost/geometry/srs/projections/impl/pj_ellps.hpp>
#include <boost/geometry/srs/projections/impl/pj_param.hpp>
#include <boost/geometry/srs/projections/proj4.hpp>
#include <boost/geometry/srs/projections/spar.hpp>

namespace boost { namespace geometry { namespace projections {

namespace detail {

/* set ellipsoid parameters a and es */
template <typename T>
inline T SIXTH() { return .1666666666666666667; } /* 1/6 */
template <typename T>
inline T RA4() { return .04722222222222222222; } /* 17/360 */
template <typename T>
inline T RA6() { return .02215608465608465608; } /* 67/3024 */
template <typename T>
inline T RV4() { return .06944444444444444444; } /* 5/72 */
template <typename T>
inline T RV6() { return .04243827160493827160; } /* 55/1296 */

template <typename T>
inline T pj_ell_b_to_es(T const& a, T const& b)
{
    return 1. - (b * b) / (a * a);
}

/************************************************************************/
/*                          pj_ell_init_ellps()                         */
/************************************************************************/

// Originally a part of pj_ell_set()
template <typename T>
inline bool pj_ell_init_ellps(srs::detail::proj4_parameters const& params, T &a, T &b)
{
    /* check if ellps present and temporarily append its values to pl */
    std::string name = pj_get_param_s(params, "ellps");
    if (! name.empty())
    {
        const pj_ellps_type<T>* pj_ellps = pj_get_ellps<T>().first;
        const int n = pj_get_ellps<T>().second;
        int index = -1;
        for (int i = 0; i < n && index == -1; i++)
        {
            if(pj_ellps[i].id == name)
            {
                index = i;
            }
        }

        if (index == -1) {
            BOOST_THROW_EXCEPTION( projection_exception(error_unknown_ellp_param) );
        }

        pj_ellps_type<T> const& pj_ellp = pj_ellps[index];
        a = pj_ellp.a;
        b = pj_ellp.b;

        return true;
    }

    return false;
}

template <typename T>
inline bool pj_ell_init_ellps(srs::dpar::parameters<T> const& params, T &a, T &b)
{
    /* check if ellps present and temporarily append its values to pl */
    typename srs::dpar::parameters<T>::const_iterator
        it = pj_param_find(params, srs::dpar::ellps);
    if (it != params.end())
    {
        if (it->template is_value_set<int>())
        {
            const pj_ellps_type<T>* pj_ellps = pj_get_ellps<T>().first;
            const int n = pj_get_ellps<T>().second;
            int i = it->template get_value<int>();

            if (i < 0 || i >= n) {
                BOOST_THROW_EXCEPTION( projection_exception(error_unknown_ellp_param) );
            }

            pj_ellps_type<T> const& pj_ellp = pj_ellps[i];
            a = pj_ellp.a;
            b = pj_ellp.b;
        }
        else if (it->template is_value_set<T>())
        {
            a = it->template get_value<T>();
            b = a;
        }
        else if (it->template is_value_set<srs::spheroid<T> >())
        {
            srs::spheroid<T> const& s = it->template get_value<srs::spheroid<T> >();
            a = geometry::get_radius<0>(s);
            b = geometry::get_radius<2>(s);
        }
        else
        {
            BOOST_THROW_EXCEPTION( projection_exception(error_unknown_ellp_param) );
        }

        return true;
    }

    return false;
}

template
<
    typename Params,
    int I = srs::spar::detail::tuples_find_index_if
        <
            Params,
            srs::spar::detail::is_param_tr<srs::spar::detail::ellps_traits>::pred
        >::value,
    int N = boost::tuples::length<Params>::value
>
struct pj_ell_init_ellps_static
{
    template <typename T>
    static bool apply(Params const& params, T &a, T &b)
    {
        typedef typename boost::tuples::element<I, Params>::type param_type;
        typedef srs::spar::detail::ellps_traits<param_type> traits_type;
        typedef typename traits_type::template model_type<T>::type model_type;

        param_type const& param = boost::tuples::get<I>(params);
        model_type const& model = traits_type::template model<T>(param);

        a = geometry::get_radius<0>(model);
        b = geometry::get_radius<2>(model);

        return true;
    }
};
template <typename Params, int N>
struct pj_ell_init_ellps_static<Params, N, N>
{
    template <typename T>
    static bool apply(Params const& , T & , T & )
    {
        return false;
    }
};

template <typename T, BOOST_GEOMETRY_PROJECTIONS_DETAIL_TYPENAME_PX>
inline bool pj_ell_init_ellps(srs::spar::parameters<BOOST_GEOMETRY_PROJECTIONS_DETAIL_PX> const& params,
                              T &a, T &b)
{
    return pj_ell_init_ellps_static
        <
            srs::spar::parameters<BOOST_GEOMETRY_PROJECTIONS_DETAIL_PX>
        >::apply(params, a, b);
}

/************************************************************************/
/*                             pj_ell_init()                            */
/************************************************************************/

/* initialize geographic shape parameters */
// This function works differently than the original pj_ell_set().
// It doesn't push parameters defined in ellps into params list.
// Instead it tries to use size (a, R) and shape (es, e, rf, f, b) parameters
// and then if needed falls back to ellps, then to datum and then to the default WGS84
template <typename Params, typename T>
inline void pj_ell_init(Params const& params, T &a, T &es)
{
    /* check for varying forms of ellipsoid input */
    a = es = 0.;

    /* R takes precedence */
    if (pj_param_f<srs::spar::r>(params, "R", srs::dpar::r, a)) {
        /* empty */
    } else { /* probable elliptical figure */

        // Set ellipsoid's size parameter
        a = pj_get_param_f<T, srs::spar::a>(params, "a", srs::dpar::a);
        bool is_a_set = a != 0.0;

        // Set ellipsoid's shape parameter
        T b = 0.0;
        bool is_ell_set = false;
        if (pj_param_f<srs::spar::es>(params, "es", srs::dpar::es, es)) {/* eccentricity squared */
            /* empty */
            is_ell_set = true;
        } else if (pj_param_f<srs::spar::e>(params, "e", srs::dpar::e, es)) { /* eccentricity */
            es = es * es;
            is_ell_set = true;
        } else if (pj_param_f<srs::spar::rf>(params, "rf", srs::dpar::rf, es)) { /* recip flattening */
            if (es == 0.0) {
                BOOST_THROW_EXCEPTION( projection_exception(error_rev_flattening_is_zero) );
            }
            es = 1./ es;
            es = es * (2. - es);
            is_ell_set = true;
        } else if (pj_param_f<srs::spar::f>(params, "f", srs::dpar::f, es)) { /* flattening */
            es = es * (2. - es);
            is_ell_set = true;
        } else if (pj_param_f<srs::spar::b>(params, "b", srs::dpar::b, b)) { /* minor axis */
            es = pj_ell_b_to_es(a, b);
            is_ell_set = true;
        } /* else es == 0. and sphere of radius a */

        // NOTE: Below when ellps is used to initialize a and es
        // b is not set because it only has sense together with a
        // but a could have been set separately before, e.g. consider passing:
        // a=1 ellps=airy (a=6377563.396 b=6356256.910)
        // after setting size parameter a and shape parameter from ellps
        // b has to be recalculated

        // If ellipsoid's parameters are not set directly
        //   use ellps parameter
        if (! is_a_set || ! is_ell_set) {
            T ellps_a = 0, ellps_b = 0;
            if (pj_ell_init_ellps(params, ellps_a, ellps_b)) {
                if (! is_a_set) {
                    a = ellps_a;
                    is_a_set = true;
                }
                if (! is_ell_set) {
                    es = pj_ell_b_to_es(ellps_a, ellps_b);
                    is_ell_set = true;
                }
            }
        }

        // If ellipsoid's parameters are not set
        //   use ellps defined by datum parameter
        if (! is_a_set || ! is_ell_set)
        {
            const pj_datums_type<T>* datum = pj_datum_find_datum<T>(params);
            if (datum != NULL)
            {
                pj_ellps_type<T> const& pj_ellp = pj_get_ellps<T>().first[datum->ellps];
                if (! is_a_set) {
                    a = pj_ellp.a;
                    is_a_set = true;
                }
                if (! is_ell_set) {
                    es = pj_ell_b_to_es(pj_ellp.a, pj_ellp.b);
                    is_ell_set = true;
                }
            }
        }

        // If ellipsoid's parameters are still not set
        //   use default WGS84
        if ((! is_a_set || ! is_ell_set)
         && ! pj_get_param_b<srs::spar::no_defs>(params, "no_defs", srs::dpar::no_defs))
        {
            pj_ellps_type<T> const& pj_ellp = pj_get_ellps<T>().first[srs::dpar::ellps_wgs84];
            if (! is_a_set) {
                a = pj_ellp.a;
                is_a_set = true;
            }
            if (! is_ell_set) {
                es = pj_ell_b_to_es(pj_ellp.a, pj_ellp.b);
                is_ell_set = true;
            }
        }

        if (b == 0.0)
            b = a * sqrt(1. - es);

        /* following options turn ellipsoid into equivalent sphere */
        if (pj_get_param_b<srs::spar::r_au>(params, "R_A", srs::dpar::r_au)) { /* sphere--area of ellipsoid */
            a *= 1. - es * (SIXTH<T>() + es * (RA4<T>() + es * RA6<T>()));
            es = 0.;
        } else if (pj_get_param_b<srs::spar::r_v>(params, "R_V", srs::dpar::r_v)) { /* sphere--vol. of ellipsoid */
            a *= 1. - es * (SIXTH<T>() + es * (RV4<T>() + es * RV6<T>()));
            es = 0.;
        } else if (pj_get_param_b<srs::spar::r_a>(params, "R_a", srs::dpar::r_a)) { /* sphere--arithmetic mean */
            a = .5 * (a + b);
            es = 0.;
        } else if (pj_get_param_b<srs::spar::r_g>(params, "R_g", srs::dpar::r_g)) { /* sphere--geometric mean */
            a = sqrt(a * b);
            es = 0.;
        } else if (pj_get_param_b<srs::spar::r_h>(params, "R_h", srs::dpar::r_h)) { /* sphere--harmonic mean */
            a = 2. * a * b / (a + b);
            es = 0.;
        } else {
            T tmp;
            bool i = pj_param_r<srs::spar::r_lat_a>(params, "R_lat_a", srs::dpar::r_lat_a, tmp);
            if (i || /* sphere--arith. */
                pj_param_r<srs::spar::r_lat_g>(params, "R_lat_g", srs::dpar::r_lat_g, tmp)) { /* or geom. mean at latitude */

                tmp = sin(tmp);
                if (geometry::math::abs(tmp) > geometry::math::half_pi<T>()) {
                    BOOST_THROW_EXCEPTION( projection_exception(error_ref_rad_larger_than_90) );
                }
                tmp = 1. - es * tmp * tmp;
                a *= i ? .5 * (1. - es + tmp) / ( tmp * sqrt(tmp)) :
                    sqrt(1. - es) / tmp;
                es = 0.;
            }
        }
    }

    /* some remaining checks */
    if (es < 0.) {
        BOOST_THROW_EXCEPTION( projection_exception(error_es_less_than_zero) );
    }
    if (a <= 0.) {
        BOOST_THROW_EXCEPTION( projection_exception(error_major_axis_not_given) );
    }
}

template <typename Params>
struct static_srs_tag_check_nonexpanded
{
    typedef typename boost::mpl::if_c
        <
            srs::spar::detail::tuples_exists_if
                <
                    Params, srs::spar::detail::is_param_t<srs::spar::r>::pred
                >::value
         || srs::spar::detail::tuples_exists_if
                <
                    Params, srs::spar::detail::is_param<srs::spar::r_au>::pred
                >::value
         || srs::spar::detail::tuples_exists_if
                <
                    Params, srs::spar::detail::is_param<srs::spar::r_v>::pred
                >::value
         || srs::spar::detail::tuples_exists_if
                <
                    Params, srs::spar::detail::is_param<srs::spar::r_a>::pred
                >::value
         || srs::spar::detail::tuples_exists_if
                <
                    Params, srs::spar::detail::is_param<srs::spar::r_g>::pred
                >::value
         || srs::spar::detail::tuples_exists_if
                <
                    Params, srs::spar::detail::is_param<srs::spar::r_h>::pred
                >::value
         || srs::spar::detail::tuples_exists_if
                <
                    Params, srs::spar::detail::is_param_t<srs::spar::r_lat_a>::pred
                >::value
         || srs::spar::detail::tuples_exists_if
                <
                    Params, srs::spar::detail::is_param_t<srs::spar::r_lat_g>::pred
                >::value,
            srs_sphere_tag,
            // NOTE: The assumption here is that if the user defines either one of:
            // b, es, e, f, rf parameters then he wants to define spheroid, not sphere
            typename boost::mpl::if_c
                <
                    srs::spar::detail::tuples_exists_if
                        <
                            Params, srs::spar::detail::is_param_t<srs::spar::b>::pred
                        >::value
                 || srs::spar::detail::tuples_exists_if
                        <
                            Params, srs::spar::detail::is_param_t<srs::spar::es>::pred
                        >::value
                 || srs::spar::detail::tuples_exists_if
                        <
                            Params, srs::spar::detail::is_param_t<srs::spar::e>::pred
                        >::value
                 || srs::spar::detail::tuples_exists_if
                        <
                            Params, srs::spar::detail::is_param_t<srs::spar::rf>::pred
                        >::value
                 || srs::spar::detail::tuples_exists_if
                        <
                            Params, srs::spar::detail::is_param_t<srs::spar::f>::pred
                        >::value,
                    srs_spheroid_tag,
                    void
                >::type
        >::type type;
};

template <typename Params>
struct static_srs_tag_check_ellps
{
    typedef typename geometry::tag
        <
            typename srs::spar::detail::ellps_traits
                <
                    typename srs::spar::detail::tuples_find_if
                        <
                            Params,
                            srs::spar::detail::is_param_tr<srs::spar::detail::ellps_traits>::pred
                        >::type
                >::template model_type<double>::type // dummy type
        >::type type;
};

template <typename Params>
struct static_srs_tag_check_datum
{
    typedef typename geometry::tag
        <
            typename srs::spar::detail::ellps_traits
                <
                    typename srs::spar::detail::datum_traits
                        <
                            typename srs::spar::detail::tuples_find_if
                                <
                                    Params,
                                    srs::spar::detail::is_param_tr<srs::spar::detail::datum_traits>::pred
                                >::type
                        >::ellps_type
                >::template model_type<double>::type // dummy type
        >::type type;
};

template
<
    typename Params,
    typename NonExpandedTag = typename static_srs_tag_check_nonexpanded
                                <
                                    Params
                                >::type,
    typename EllpsTag = typename static_srs_tag_check_ellps
                            <
                                Params
                            >::type,
    typename DatumTag = typename static_srs_tag_check_datum
                            <
                                Params
                            >::type
>
struct static_srs_tag
{
    // User passed one of the non-ellps, non-datum parameters
    typedef NonExpandedTag type;
};

template <typename Params, typename EllpsTag, typename DatumTag>
struct static_srs_tag<Params, void, EllpsTag, DatumTag>
{
    // User didn't pass neither one of the non-ellps, non-datum parameters
    // but passed ellps
    typedef EllpsTag type;
};

template <typename Params, typename DatumTag>
struct static_srs_tag<Params, void, void, DatumTag>
{
    // User didn't pass neither one of the non-ellps, non-datum parameters
    // nor ellps parameter but passed datum parameter
    typedef DatumTag type;
};

template <typename Params>
struct static_srs_tag<Params, void, void, void>
{
    // User didn't pass any parameter defining model
    // so use default or generate error
    typedef typename boost::mpl::if_c
        <
            srs::spar::detail::tuples_exists_if
                <
                    Params, srs::spar::detail::is_param<srs::spar::no_defs>::pred
                >::value,
            void,
            srs_spheroid_tag // WGS84
        >::type type;

    static const bool is_found = ! boost::is_same<type, void>::value;
    BOOST_MPL_ASSERT_MSG((is_found), UNKNOWN_ELLP_PARAM, (Params));
};

template <typename T>
inline void pj_calc_ellipsoid_params(parameters<T> & p, T const& a, T const& es) {
/****************************************************************************************
    Calculate a large number of ancillary ellipsoidal parameters, in addition to
    the two traditional PROJ defining parameters: Semimajor axis, a, and the
    eccentricity squared, es.

    Most of these parameters are fairly cheap to compute in comparison to the overall
    effort involved in initializing a PJ object. They may, however, take a substantial
    part of the time taken in computing an individual point transformation.

    So by providing them up front, we can amortize the (already modest) cost over all
    transformations carried out over the entire lifetime of a PJ object, rather than
    incur that cost for every single transformation.

    Most of the parameter calculations here are based on the "angular eccentricity",
    i.e. the angle, measured from the semiminor axis, of a line going from the north
    pole to one of the foci of the ellipsoid - or in other words: The arc sine of the
    eccentricity.

    The formulae used are mostly taken from:

    Richard H. Rapp: Geometric Geodesy, Part I, (178 pp, 1991).
    Columbus, Ohio:  Dept. of Geodetic Science
    and Surveying, Ohio State University.

****************************************************************************************/

    p.a = a;
    p.es = es;

    /* Compute some ancillary ellipsoidal parameters */
    if (p.e==0)
        p.e = sqrt(p.es);  /* eccentricity */
    //p.alpha = asin (p.e);  /* angular eccentricity */

    /* second eccentricity */
    //p.e2  = tan (p.alpha);
    //p.e2s = p.e2 * p.e2;

    /* third eccentricity */
    //p.e3    = (0!=p.alpha)? sin (p.alpha) / sqrt(2 - sin (p.alpha)*sin (p.alpha)): 0;
    //p.e3s = p.e3 * p.e3;

    /* flattening */
    //if (0==p.f)
    //    p.f  = 1 - cos (p.alpha);   /* = 1 - sqrt (1 - PIN->es); */
    //p.rf = p.f != 0.0 ? 1.0/p.f: HUGE_VAL;

    /* second flattening */
    //p.f2  = (cos(p.alpha)!=0)? 1/cos (p.alpha) - 1: 0;
    //p.rf2 = p.f2 != 0.0 ? 1/p.f2: HUGE_VAL;

    /* third flattening */
    //p.n  = pow (tan (p.alpha/2), 2);
    //p.rn = p.n != 0.0 ? 1/p.n: HUGE_VAL;

    /* ...and a few more */
    //if (0==p.b)
    //    p.b  = (1 - p.f)*p.a;
    //p.rb = 1. / p.b;
    p.ra = 1. / p.a;

    p.one_es = 1. - p.es;
    if (p.one_es == 0.) {
        BOOST_THROW_EXCEPTION( projection_exception(error_eccentricity_is_one) );
    }

    p.rone_es = 1./p.one_es;
}

} // namespace detail
}}} // namespace boost::geometry::projections

#endif // BOOST_GEOMETRY_PROJECTIONS_IMPL_PJ_ELL_SET_HPP


#include "drake/solvers/sdpa_free_format.h"

#include <algorithm>
#include <fstream>
#include <initializer_list>
#include <iomanip>
#include <limits>
#include <stdexcept>
#include <unordered_map>
#include <vector>

#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Eigen/SparseQR>

namespace drake {
namespace solvers {
namespace internal {

const double kInf = std::numeric_limits<double>::infinity();

SdpaFreeFormat::~SdpaFreeFormat() {}

void SdpaFreeFormat::DeclareXforPositiveSemidefiniteConstraints(
    const MathematicalProgram& prog,
    std::unordered_map<symbolic::Variable::Id, std::vector<EntryInX>>*
        entries_in_X_for_same_decision_variable) {
  int num_blocks = 0;
  for (const auto& binding : prog.positive_semidefinite_constraints()) {
    // The bound variables are the column-stacked vector of the psd matrix. We
    // only record the upper-diagonal entries in the psd matrix.
    int psd_matrix_variable_index = 0;
    const int matrix_rows = binding.evaluator()->matrix_rows();
    X_blocks_.emplace_back(BlockType::kMatrix, matrix_rows);
    for (int j = 0; j < matrix_rows; ++j) {
      for (int i = 0; i <= j; ++i) {
        const symbolic::Variable& psd_ij_var =
            binding.variables()(psd_matrix_variable_index++);
        const int psd_ij_var_index = prog.FindDecisionVariableIndex(psd_ij_var);
        const bool has_var_registered =
            !(std::holds_alternative<std::nullptr_t>(
                prog_var_in_sdpa_[psd_ij_var_index]));
        const EntryInX psd_ij_entry_in_X(num_blocks, i, j, num_X_rows_);
        if (!has_var_registered) {
          // This variable has not been registered into X. Now register this
          // variable, by adding it to prog_var_in_sdpa_
          prog_var_in_sdpa_[psd_ij_var_index].emplace<DecisionVariableInSdpaX>(
              Sign::kPositive, 0, psd_ij_entry_in_X);
        } else {
          // This variable has been registered into X. We need to add the
          // equality constraint between all X entries corresponding to this
          // variable.

          // First find if there exists equality constraint on this variable
          // already.
          const auto it2 = entries_in_X_for_same_decision_variable->find(
              psd_ij_var.get_id());
          if (it2 == entries_in_X_for_same_decision_variable->end()) {
            // There does not exist equality constraint on this variable yet.
            entries_in_X_for_same_decision_variable->emplace_hint(
                it2, psd_ij_var.get_id(),
                std::vector<EntryInX>({std::get<DecisionVariableInSdpaX>(
                                           prog_var_in_sdpa_[psd_ij_var_index])
                                           .entry_in_X,
                                       psd_ij_entry_in_X}));
          } else {
            // We found equality constraint on this variable, append the vector
            // containing all X entries that correspond to this variable.
            it2->second.push_back(psd_ij_entry_in_X);
          }
        }
      }
      psd_matrix_variable_index += matrix_rows - j - 1;
    }
    num_blocks++;
    num_X_rows_ += matrix_rows;
  }
}

void AddTermToTriplets(const EntryInX& entry_in_X, double coeff,
                       std::vector<Eigen::Triplet<double>>* triplets) {
  if (entry_in_X.row_index_in_block == entry_in_X.column_index_in_block) {
    triplets->emplace_back(
        entry_in_X.X_start_row + entry_in_X.row_index_in_block,
        entry_in_X.X_start_row + entry_in_X.column_index_in_block, coeff);
  } else {
    triplets->emplace_back(
        entry_in_X.X_start_row + entry_in_X.row_index_in_block,
        entry_in_X.X_start_row + entry_in_X.column_index_in_block, coeff / 2);
    triplets->emplace_back(
        entry_in_X.X_start_row + entry_in_X.column_index_in_block,
        entry_in_X.X_start_row + entry_in_X.row_index_in_block, coeff / 2);
  }
}

void SdpaFreeFormat::AddLinearEqualityConstraint(
    const std::vector<double>& coeff_prog_vars,
    const std::vector<int>& prog_vars_indices,
    const std::vector<double>& coeff_X_entries,
    const std::vector<EntryInX>& X_entries,
    const std::vector<double>& coeff_free_vars,
    const std::vector<FreeVariableIndex>& free_vars_indices, double rhs) {
  DRAKE_ASSERT(coeff_prog_vars.size() == prog_vars_indices.size());
  DRAKE_ASSERT(coeff_X_entries.size() == X_entries.size());
  DRAKE_ASSERT(coeff_free_vars.size() == free_vars_indices.size());
  const int constraint_index = static_cast<int>(A_triplets_.size());
  std::vector<Eigen::Triplet<double>> Ai_triplets;
  // If the entries are off-diagonal entries of X, then it needs two
  // coefficients in Ai, for both the upper and lower diagonal part.
  Ai_triplets.reserve(
      static_cast<int>(coeff_prog_vars.size() + coeff_X_entries.size()) * 2);
  g_.conservativeResize(g_.rows() + 1);
  g_(constraint_index) = rhs;
  for (int i = 0; i < static_cast<int>(coeff_prog_vars.size()); ++i) {
    if (coeff_prog_vars[i] != 0) {
      const int prog_var_index = prog_vars_indices[i];
      if (std::holds_alternative<DecisionVariableInSdpaX>(
              prog_var_in_sdpa_[prog_var_index])) {
        // This variable is an entry in X.
        const auto& decision_var_in_X =
            std::get<DecisionVariableInSdpaX>(
                prog_var_in_sdpa_[prog_var_index]);
        g_(constraint_index) -= coeff_prog_vars[i] * decision_var_in_X.offset;
        const double coeff = decision_var_in_X.coeff_sign == Sign::kPositive
                                 ? coeff_prog_vars[i]
                                 : -coeff_prog_vars[i];
        AddTermToTriplets(decision_var_in_X.entry_in_X, coeff, &Ai_triplets);
      } else if (std::holds_alternative<double>(
          prog_var_in_sdpa_[prog_var_index])) {
        // This variable has a constant value.
        const double var_value = std::get<double>(
            prog_var_in_sdpa_[prog_var_index]);
        g_(constraint_index) -= coeff_prog_vars[i] * var_value;
      } else if (std::holds_alternative<FreeVariableIndex>(
                     prog_var_in_sdpa_[prog_var_index])) {
        // This variable is a free variable (no lower nor upper bound).
        B_triplets_.emplace_back(
            constraint_index,
            std::get<FreeVariableIndex>(prog_var_in_sdpa_[prog_var_index]),
            coeff_prog_vars[i]);
      } else {
        throw std::runtime_error(
            "SdpaFreeFormat::AddLinearEqualityConstraint() : this decision "
            "variable is not an entry in X or s, and is not a constant.");
      }
    }
  }
  // Adds coeff_X_entries * X_entries.
  for (int i = 0; i < static_cast<int>(coeff_X_entries.size()); ++i) {
    if (coeff_X_entries[i] != 0) {
      AddTermToTriplets(X_entries[i], coeff_X_entries[i], &Ai_triplets);
    }
  }
  A_triplets_.push_back(Ai_triplets);
  // Adds coeff_free_vars * free_vars.
  if (!coeff_X_entries.empty()) {
    for (int i = 0; i < static_cast<int>(coeff_free_vars.size()); ++i) {
      B_triplets_.emplace_back(constraint_index, free_vars_indices[i],
                               coeff_free_vars[i]);
    }
  }
}

void SdpaFreeFormat::AddEqualityConstraintOnXEntriesForSameDecisionVariable(
    const std::unordered_map<symbolic::Variable::Id, std::vector<EntryInX>>&
        entries_in_X_for_same_decision_variable) {
  for (const auto& item : entries_in_X_for_same_decision_variable) {
    const auto& Xentries = item.second;
    DRAKE_ASSERT(Xentries.size() >= 2);
    const std::vector<double> a{{1, -1}};
    for (int i = 1; i < static_cast<int>(Xentries.size()); ++i) {
      AddLinearEqualityConstraint({} /* coefficients for prog_vars */,
                                  {} /* empty prog_vars */, a,
                                  {Xentries[0], Xentries[i]}, {}, {}, 0.0);
    }
  }
}

void SdpaFreeFormat::RegisterSingleMathematicalProgramDecisionVariable(
    double lower_bound, double upper_bound, int variable_index, int block_index,
    int* new_X_var_count) {
  // First check if this variable has either finite lower or upper bound. If
  // not, then the variable is a free variable in SDPA.
  if (std::isinf(lower_bound) && std::isinf(upper_bound)) {
    // This is a free variable.
    prog_var_in_sdpa_[variable_index].emplace<FreeVariableIndex>(
        num_free_variables_);
    num_free_variables_++;
  } else {
    if (!std::isinf(lower_bound) && std::isinf(upper_bound)) {
      // lower <= x.
      prog_var_in_sdpa_[variable_index].emplace<DecisionVariableInSdpaX>(
          Sign::kPositive, lower_bound, block_index, *new_X_var_count,
          *new_X_var_count, num_X_rows_);
      (*new_X_var_count)++;
    } else if (std::isinf(lower_bound) && !std::isinf(upper_bound)) {
      // x <= upper.
      prog_var_in_sdpa_[variable_index].emplace<DecisionVariableInSdpaX>(
          Sign::kNegative, upper_bound, block_index, *new_X_var_count,
          *new_X_var_count, num_X_rows_);
      (*new_X_var_count)++;
    } else if (lower_bound == upper_bound) {
      // x == bound.
      prog_var_in_sdpa_[variable_index].emplace<double>(lower_bound);
    } else {
      // lower <= x <= upper.
      // x will be replaced with y + lower, where y is an entry in X.
      prog_var_in_sdpa_[variable_index].emplace<DecisionVariableInSdpaX>(
          Sign::kPositive, lower_bound, block_index, *new_X_var_count,
          *new_X_var_count, num_X_rows_);
      // Add another slack variables z, with the constraint x + z = upper.
      AddLinearEqualityConstraint({1}, {variable_index}, {1.0},
                                  {EntryInX(block_index, *new_X_var_count + 1,
                                            *new_X_var_count + 1, num_X_rows_)},
                                  {}, {}, upper_bound);
      (*new_X_var_count) += 2;
    }
  }
}

void SdpaFreeFormat::AddBoundsOnRegisteredDecisionVariable(
    double lower_bound, double upper_bound, int variable_index, int block_index,
    int* new_X_var_count) {
  // This variable has been registered as a variable in SDPA. It should have
  // been registered as an entry in X.
  if (std::holds_alternative<DecisionVariableInSdpaX>(
          prog_var_in_sdpa_[variable_index])) {
    if (std::isinf(lower_bound) && std::isinf(upper_bound)) {
      // no finite bounds.
      return;
    } else if (!std::isinf(lower_bound) && std::isinf(upper_bound)) {
      // lower <= x.
      // Adds a slack variable y as a diagonal entry in X, with the
      // constraint x - y = lower.
      AddLinearEqualityConstraint({1}, {variable_index}, {-1.0},
                                  {EntryInX(block_index, *new_X_var_count,
                                            *new_X_var_count, num_X_rows_)},
                                  {}, {}, lower_bound);
      (*new_X_var_count)++;
    } else if (std::isinf(lower_bound) && !std::isinf(upper_bound)) {
      // x <= upper.
      // Adds a slack variable y as a diagonal entry in X, with the
      // constraint x + y = upper.
      AddLinearEqualityConstraint({1.0}, {variable_index}, {1.0},
                                  {EntryInX(block_index, *new_X_var_count,
                                            *new_X_var_count, num_X_rows_)},
                                  {}, {}, upper_bound);
      (*new_X_var_count)++;
    } else if (lower_bound == upper_bound) {
      // Add the constraint x == bound.
      AddLinearEqualityConstraint({1.0}, {variable_index}, {}, {}, {}, {},
                                  lower_bound);
    } else {
      // lower <= x <= upper.
      // Adds two slack variables y1, y2 in X, with the linear equality
      // constraint x - y1 = lower and x + y2 <= upper.
      AddLinearEqualityConstraint({1.0}, {variable_index}, {-1.0},
                                  {EntryInX(block_index, *new_X_var_count,
                                            *new_X_var_count, num_X_rows_)},
                                  {}, {}, lower_bound);
      AddLinearEqualityConstraint(
          {1.0}, {variable_index}, {1.0},
          {EntryInX(block_index, (*new_X_var_count) + 1, (*new_X_var_count) + 1,
                    num_X_rows_)},
          {}, {}, upper_bound);
      (*new_X_var_count) += 2;
    }
  } else {
    throw std::runtime_error(
        "SdpaFreeFormat::AddBoundsOnRegisteredDecisionVariable(): "
        "the registered variable should be an entry in X.");
  }
}

void SdpaFreeFormat::RegisterMathematicalProgramDecisionVariables(
    const MathematicalProgram& prog) {
  // Go through all the decision variables in @p prog. If the decision variable
  // has not been registered in SDPA, then register it now. Refer to @ref
  // prog_var_in_sdpa_ for different types.
  Eigen::VectorXd lower_bound =
      Eigen::VectorXd::Constant(prog.num_vars(), -kInf);
  Eigen::VectorXd upper_bound =
      Eigen::VectorXd::Constant(prog.num_vars(), kInf);
  for (const auto& bounding_box : prog.bounding_box_constraints()) {
    for (int i = 0; i < bounding_box.variables().rows(); ++i) {
      const int variable_index =
          prog.FindDecisionVariableIndex(bounding_box.variables()(i));
      lower_bound(variable_index) =
          std::max(lower_bound(variable_index),
                   bounding_box.evaluator()->lower_bound()(i));
      upper_bound(variable_index) =
          std::min(upper_bound(variable_index),
                   bounding_box.evaluator()->upper_bound()(i));
    }
  }

  // Now go through each of the decision variables in prog.

  const int block_index = static_cast<int>(X_blocks_.size());
  int new_X_var_count = 0;
  for (int i = 0; i < prog.num_vars(); ++i) {
    // The variable might have been registered when we go through the positive
    // semidefinite constraint.
    bool has_var_registered =
        !std::holds_alternative<std::nullptr_t>(prog_var_in_sdpa_[i]);
    // First check if this variable has either finite lower or upper bound. If
    // not, then the variable is a free variable in SDPA.
    if (!has_var_registered) {
      RegisterSingleMathematicalProgramDecisionVariable(

          lower_bound(i), upper_bound(i), i, block_index, &new_X_var_count);
    } else {
      AddBoundsOnRegisteredDecisionVariable(lower_bound(i), upper_bound(i), i,
                                            block_index, &new_X_var_count);
    }
  }
  if (new_X_var_count > 0) {
    X_blocks_.emplace_back(BlockType::kDiagonal, new_X_var_count);
    num_X_rows_ += new_X_var_count;
  }
}

void SdpaFreeFormat::AddLinearCostsFromProgram(
    const MathematicalProgram& prog) {
  for (const auto& linear_cost : prog.linear_costs()) {
    for (int i = 0; i < linear_cost.variables().rows(); ++i) {
      // The negation sign is because in SDPA format, the objective is to
      // maximize the cost, while in MathematicalProgram, the objective is to
      // minimize the cost.
      double coeff = -linear_cost.evaluator()->a()(i);
      if (coeff != 0) {
        // Only adds the cost if the cost coefficient is non-zero.
        const int var_index =
            prog.FindDecisionVariableIndex(linear_cost.variables()(i));
        if (std::holds_alternative<DecisionVariableInSdpaX>(
                prog_var_in_sdpa_[var_index])) {
          const auto& decision_var_in_X =
              std::get<DecisionVariableInSdpaX>(prog_var_in_sdpa_[var_index]);
          coeff =
              decision_var_in_X.coeff_sign == Sign::kPositive ? coeff : -coeff;
          constant_min_cost_term_ +=
              linear_cost.evaluator()->a()(i) * decision_var_in_X.offset;
          AddTermToTriplets(decision_var_in_X.entry_in_X, coeff, &C_triplets_);
        } else if (std::holds_alternative<double>(
            prog_var_in_sdpa_[var_index])) {
          const double val = std::get<double>(prog_var_in_sdpa_[var_index]);
          constant_min_cost_term_ += linear_cost.evaluator()->a()(i) * val;
        } else if (std::holds_alternative<FreeVariableIndex>(
                       prog_var_in_sdpa_[var_index])) {
          const FreeVariableIndex& s_index =
              std::get<FreeVariableIndex>(prog_var_in_sdpa_[var_index]);
          d_triplets_.emplace_back(s_index, 0, coeff);
        } else {
          throw std::runtime_error(
              "SdpaFreeFormat::AddLinearCostFromProgram() only supports "
              "DecisionVariableInSdpaX, double or FreeVariableIndex.");
        }
      }
    }
    constant_min_cost_term_ += linear_cost.evaluator()->b();
  }
}

template <typename Constraint>
void SdpaFreeFormat::AddLinearConstraintsHelper(
    const MathematicalProgram& prog,
    const Binding<Constraint>& linear_constraint, bool is_equality_constraint,
    int* linear_constraint_slack_entry_in_X_count) {
  const std::vector<int> var_indices =
      prog.FindDecisionVariableIndices(linear_constraint.variables());
  // Go through each row of the constraint.
  for (int i = 0; i < linear_constraint.evaluator()->num_constraints(); ++i) {
    const bool does_lower_equal_upper_in_this_row =
        is_equality_constraint ||
        linear_constraint.evaluator()->lower_bound()(i) ==
            linear_constraint.evaluator()->upper_bound()(i);
    std::vector<double> a, b;
    // If this row's lower and upper bound are the same, then we only need to
    // append one row to SDPA format, namely tr(Ai * X) + bi' * s = rhs,
    // Otherwise, we need to append two rows to SDPA format
    // tr(Ai_lower * X_lower) + bi' * s = lower_bound
    // tr(Ai_upper * X_upper) + bi' * s = upper_bound
    // and append two diagonal entries into X as slack variables.
    a.reserve(var_indices.size());
    b.reserve(1);
    std::vector<int> decision_var_indices_in_X;
    decision_var_indices_in_X.reserve(var_indices.size());
    std::vector<EntryInX> X_entries;
    X_entries.reserve(1);
    std::vector<int> s_indices;
    for (int j = 0; j < linear_constraint.variables().rows(); ++j) {
      if (linear_constraint.evaluator()->A()(i, j) != 0) {
        a.push_back(linear_constraint.evaluator()->A()(i, j));
        decision_var_indices_in_X.push_back(var_indices[j]);
      }
    }
    if (does_lower_equal_upper_in_this_row) {
      // Add the equality constraint.
      AddLinearEqualityConstraint(
          a, decision_var_indices_in_X, {}, {}, {}, {},
          linear_constraint.evaluator()->lower_bound()(i));
    } else {
      // First add the constraint and slack variable for the lower bound.
      if (!std::isinf(linear_constraint.evaluator()->lower_bound()(i))) {
        AddLinearEqualityConstraint(
            a, decision_var_indices_in_X, {-1},
            {EntryInX(static_cast<int>(X_blocks_.size()),
                      *linear_constraint_slack_entry_in_X_count,
                      *linear_constraint_slack_entry_in_X_count, num_X_rows_)},
            {}, {}, linear_constraint.evaluator()->lower_bound()(i));
        (*linear_constraint_slack_entry_in_X_count)++;
      }
      // Now add the constraint and slack variable for the upper bound.
      if (!std::isinf(linear_constraint.evaluator()->upper_bound()(i))) {
        AddLinearEqualityConstraint(
            a, decision_var_indices_in_X, {1},
            {EntryInX(static_cast<int>(X_blocks_.size()),
                      *linear_constraint_slack_entry_in_X_count,
                      *linear_constraint_slack_entry_in_X_count, num_X_rows_)},
            {}, {}, linear_constraint.evaluator()->upper_bound()(i));
        (*linear_constraint_slack_entry_in_X_count)++;
      }
    }
  }
}

void SdpaFreeFormat::AddLinearConstraintsFromProgram(
    const MathematicalProgram& prog) {
  int linear_constraint_slack_entry_in_X_count = 0;
  for (const auto& linear_eq_constraint : prog.linear_equality_constraints()) {
    AddLinearConstraintsHelper(prog, linear_eq_constraint, true,
                               &linear_constraint_slack_entry_in_X_count);
  }
  for (const auto& linear_constraint : prog.linear_constraints()) {
    AddLinearConstraintsHelper(prog, linear_constraint, false,
                               &linear_constraint_slack_entry_in_X_count);
  }
  if (linear_constraint_slack_entry_in_X_count > 0) {
    num_X_rows_ += linear_constraint_slack_entry_in_X_count;
    X_blocks_.emplace_back(BlockType::kDiagonal,
                           linear_constraint_slack_entry_in_X_count);
  }
}

void SdpaFreeFormat::AddLinearMatrixInequalityConstraints(
    const MathematicalProgram& prog) {
  for (const auto& lmi_constraint :
       prog.linear_matrix_inequality_constraints()) {
    const std::vector<int> var_indices =
        prog.FindDecisionVariableIndices(lmi_constraint.variables());
    // Add the constraint that F1 * x1 + ... + Fn * xn - X_slack = -F0 and
    // X_slack is psd.
    const std::vector<Eigen::MatrixXd>& F = lmi_constraint.evaluator()->F();
    for (int j = 0; j < lmi_constraint.evaluator()->matrix_rows(); ++j) {
      for (int i = 0; i <= j; ++i) {
        std::vector<double> a;
        a.reserve(static_cast<int>(F.size()) - 1);
        for (int k = 1; k < static_cast<int>(F.size()); ++k) {
          a.push_back(F[k](i, j));
        }
        AddLinearEqualityConstraint(
            a, var_indices, {-1},
            {EntryInX(static_cast<int>(X_blocks_.size()), i, j, num_X_rows_)},
            {}, {}, -F[0](i, j));
      }
    }

    X_blocks_.emplace_back(BlockType::kMatrix,
                           lmi_constraint.evaluator()->matrix_rows());
    num_X_rows_ += lmi_constraint.evaluator()->matrix_rows();
  }
}

// A Lorentz cone constraint z₀ ≥ sqrt(z₁² + ... + zₙ²), where z = A*x+b, can
// be rewritten as a positive semidefinite constraint
// ⎡ z₀ z₁ ... zₙ ⎤
// ⎢ z₁ z₀  0  0  ⎥  is positive semidefinite.
// ⎢    ....      ⎥
// ⎣ zₙ 0   0  z₀ ⎦
void SdpaFreeFormat::AddLorentzConeConstraints(
    const MathematicalProgram& prog) {
  for (const auto& lorentz_cone_constraint : prog.lorentz_cone_constraints()) {
    const int num_block_rows = lorentz_cone_constraint.evaluator()->A().rows();
    const int num_decision_vars = lorentz_cone_constraint.variables().rows();
    const std::vector<int> prog_vars_indices =
        prog.FindDecisionVariableIndices(lorentz_cone_constraint.variables());

    // Add the linear constraint that all the diagonal terms of the new block
    // matrix equals to z0.
    std::vector<double> a;
    // The last entry in X_entries would be the diagonal term in the new block.
    a.reserve(num_decision_vars);
    // We need to impose the linear equality constraint
    // lorentz_cone_constraint.evaluator()->A().row(0) *
    // lorentz_cone_constraint.variables() - new_block(i, i) =
    // -lorentz_cone_constraint.evaluator()->b()(0).
    // So we first fill in a, b, X_entries and s_indices with the term from
    // lorentz_cone_constraint.evaluator()->A().row(0) *
    // lorentz_cone_constraint.variables()
    for (int i = 0; i < num_decision_vars; ++i) {
      const double coeff = lorentz_cone_constraint.evaluator()->A_dense()(0, i);
      a.push_back(coeff);
    }

    // For each diagonal entry in the new block matrix, we need to add
    // -new_block(i, i) to the left-hand side of the equality constraint.
    for (int i = 0; i < num_block_rows; ++i) {
      AddLinearEqualityConstraint(
          a, prog_vars_indices, {-1.0},
          {EntryInX(static_cast<int>(X_blocks_.size()), i, i, num_X_rows_)}, {},
          {}, -lorentz_cone_constraint.evaluator()->b()(0));
    }

    // Now we add the linear equality constraint arising from the first row of
    // the new block lorentz_cone_constraint.evaluator()->A().row(i) *
    // lorentz_cone_constraint.variables() +
    // lorentz_cone_constraint.evaluator()->b()(i) = new_block(0, i) for i
    for (int i = 1; i < num_block_rows; ++i) {
      a.clear();
      a.reserve(num_decision_vars);
      for (int j = 0; j < num_decision_vars; ++j) {
        const double coeff =
            lorentz_cone_constraint.evaluator()->A_dense()(i, j);
        a.push_back(coeff);
      }
      // Add the term -new_block(0, i)
      AddLinearEqualityConstraint(
          a, prog_vars_indices, {-1},
          {EntryInX(static_cast<int>(X_blocks_.size()), 0, i, num_X_rows_)}, {},
          {}, -lorentz_cone_constraint.evaluator()->b()(i));
    }

    // Now add the constraint that many entries in this new block is 0.
    for (int i = 1; i < num_block_rows; ++i) {
      for (int j = 1; j < i; ++j) {
        AddLinearEqualityConstraint(
            {}, {}, {1.0},
            {EntryInX(static_cast<int>(X_blocks_.size()), i, j, num_X_rows_)},
            {}, {}, 0);
      }
    }

    X_blocks_.emplace_back(BlockType::kMatrix, num_block_rows);
    num_X_rows_ += num_block_rows;
  }
}

// A vector z in rotated Lorentz cone (i.e.,z₀≥0, z₁≥0, z₀z₁≥ sqrt(z₂² + ...
// zₙ²), where z = Ax+b ) is equivalent to the following positive semidefinite
// constraint
// ⎡ z₀ z₂ z₃ ... zₙ ⎤
// ⎢ z₂ z₁ 0  ...  0 ⎥
// ⎢ z₃ 0 z₁ 0 ... 0 ⎥ is positive semidefinite.
// ⎢ z₄ 0 0 z₁ 0   0 ⎥
// ⎢      ...        ⎥
// ⎢ zₙ₋₁ ...   z₁ 0 ⎥
// ⎣ zₙ 0 ... 0    z₁⎦
void SdpaFreeFormat::AddRotatedLorentzConeConstraints(
    const MathematicalProgram& prog) {
  for (const auto& rotated_lorentz_constraint :
       prog.rotated_lorentz_cone_constraints()) {
    const int z_size = rotated_lorentz_constraint.evaluator()->A().rows();
    // The number of rows in the new block matrix
    const int num_block_rows = z_size - 1;
    const int num_decision_vars = rotated_lorentz_constraint.variables().rows();
    const std::vector<int> prog_vars_indices = prog.FindDecisionVariableIndices(
        rotated_lorentz_constraint.variables());
    // First add the equality constraint arising from the first row of the PSD
    // constraint. rotated_lorentz_constraint.evaluator()->A().row(j) *
    // rotated_lorentz_constraint.variables() +
    // rotated_lorentz_constraint.evaluator()->b()(j) = new_X(0, i);
    // where j = 0 if i = 0, and j = i + 1 otherwise.
    for (int i = 0; i < num_block_rows; ++i) {
      std::vector<double> a;
      a.reserve(num_decision_vars);
      const int j = i == 0 ? 0 : i + 1;
      for (int k = 0; k < num_decision_vars; ++k) {
        a.push_back(rotated_lorentz_constraint.evaluator()->A_dense()(j, k));
      }
      // Add the term -new_X(0, i)
      AddLinearEqualityConstraint(
          a, prog_vars_indices, {-1},
          {EntryInX(static_cast<int>(X_blocks_.size()), 0, i, num_X_rows_)}, {},
          {}, -rotated_lorentz_constraint.evaluator()->b()(j));
    }

    // Add the linear constraint
    // rotated_lorentz_constraint.evaluator()->A().row(1) *
    // rotated_lorentz_constraint.variables() +
    // rotated_lorentz_constraint.evaluator()->b()(1) = new_X(i, i) for i >= 1
    std::vector<double> a;
    a.reserve(num_decision_vars);
    for (int i = 0; i < num_decision_vars; ++i) {
      a.push_back(rotated_lorentz_constraint.evaluator()->A_dense()(1, i));
    }
    for (int i = 1; i < num_block_rows; ++i) {
      // Add the term -new_block(i, i).
      AddLinearEqualityConstraint(
          a, prog_vars_indices, {-1},
          {EntryInX(static_cast<int>(X_blocks_.size()), i, i, num_X_rows_)}, {},
          {}, -rotated_lorentz_constraint.evaluator()->b()(1));
    }

    // Now add the constraint that new_X(i, j) = 0 for j >= 2 and 1 <= i < j
    for (int j = 2; j < num_block_rows; ++j) {
      for (int i = 1; i < j; ++i) {
        AddLinearEqualityConstraint(
            {}, {}, {1},
            {EntryInX(static_cast<int>(X_blocks_.size()), i, j, num_X_rows_)},
            {}, {}, 0);
      }
    }

    X_blocks_.emplace_back(BlockType::kMatrix, num_block_rows);
    num_X_rows_ += num_block_rows;
  }
}

void SdpaFreeFormat::Finalize() {
  A_.reserve(A_triplets_.size());
  for (int i = 0; i < static_cast<int>(A_triplets_.size()); ++i) {
    A_.emplace_back(num_X_rows_, num_X_rows_);
    A_.back().setFromTriplets(A_triplets_[i].begin(), A_triplets_[i].end());
  }
  B_.resize(static_cast<int>(A_triplets_.size()), num_free_variables_);
  B_.setFromTriplets(B_triplets_.begin(), B_triplets_.end());
  C_.resize(num_X_rows_, num_X_rows_);
  C_.setFromTriplets(C_triplets_.begin(), C_triplets_.end());
  d_.resize(num_free_variables_, 1);
  d_.setFromTriplets(d_triplets_.begin(), d_triplets_.end());
}

SdpaFreeFormat::SdpaFreeFormat(const MathematicalProgram& prog) {
  ProgramAttributes solver_capabilities(std::initializer_list<ProgramAttribute>{
      ProgramAttribute::kLinearCost, ProgramAttribute::kLinearConstraint,
      ProgramAttribute::kLinearEqualityConstraint,
      ProgramAttribute::kLorentzConeConstraint,
      ProgramAttribute::kRotatedLorentzConeConstraint,
      ProgramAttribute::kPositiveSemidefiniteConstraint});
  if (!AreRequiredAttributesSupported(prog.required_capabilities(),
                                      solver_capabilities)) {
    throw std::invalid_argument(
        "SdpaFreeFormat(): the program cannot be formulated as an SDP "
        "in the standard form.\n");
  }
  prog_var_in_sdpa_.reserve(prog.num_vars());
  for (int i = 0; i < prog.num_vars(); ++i) {
    prog_var_in_sdpa_.emplace_back(nullptr);
  }
  std::unordered_map<symbolic::Variable::Id, std::vector<EntryInX>>
      entries_in_X_for_same_decision_variable;
  DeclareXforPositiveSemidefiniteConstraints(
      prog, &entries_in_X_for_same_decision_variable);

  AddEqualityConstraintOnXEntriesForSameDecisionVariable(
      entries_in_X_for_same_decision_variable);

  RegisterMathematicalProgramDecisionVariables(prog);

  AddLinearCostsFromProgram(prog);

  AddLinearConstraintsFromProgram(prog);

  AddLinearMatrixInequalityConstraints(prog);

  AddLorentzConeConstraints(prog);

  AddRotatedLorentzConeConstraints(prog);

  Finalize();
}

void SdpaFreeFormat::RemoveFreeVariableByNullspaceApproach(
    Eigen::SparseMatrix<double>* C_hat,
    std::vector<Eigen::SparseMatrix<double>>* A_hat, Eigen::VectorXd* rhs_hat,
    Eigen::VectorXd* y_hat,
    Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>>*
        QR_B) const {
  DRAKE_ASSERT(this->num_free_variables() != 0);
  const int num_constraints = static_cast<int>(this->A_triplets().size());
  // Do a QR decomposition on B to find the null space of Bᵀ.
  QR_B->analyzePattern(this->B());
  QR_B->factorize(this->B());
  if (QR_B->info() != Eigen::Success) {
    throw std::runtime_error(
        "SdpaFreeFormat::RemoveFreeVariableByNullspaceApproach(): cannot "
        "perform QR decomposition of B. Please try the other method "
        "kTwoSlackVariables.");
  }
  // BP = [Q₁ Q₂] * [R; 0], so the nullspace of Bᵀ is Q₂
  Eigen::SparseMatrix<double> Q;
  Q = QR_B->matrixQ();
  const Eigen::SparseMatrix<double> N =
      Q.rightCols(this->B().rows() - QR_B->rank());
  *rhs_hat = N.transpose() * this->g();

  A_hat->clear();
  A_hat->reserve(N.cols());
  for (int i = 0; i < N.cols(); ++i) {
    A_hat->emplace_back(this->num_X_rows(), this->num_X_rows());
    A_hat->back().setZero();
    for (Eigen::SparseMatrix<double>::InnerIterator it_N(N, i); it_N; ++it_N) {
      A_hat->back() += it_N.value() * this->A()[it_N.row()];
    }
  }

  const Eigen::SparseMatrix<double> B_t = this->B().transpose();
  Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>>
      qr_B_t;
  qr_B_t.compute(B_t);
  if (qr_B_t.info() != Eigen::Success) {
    throw std::runtime_error(
        "RemoveFreeVariableByNullspaceApproach():QR "
        "decomposition on B.transpose() fails\n");
  }
  *y_hat = qr_B_t.solve(Eigen::VectorXd(this->d()));
  *C_hat = this->C();
  for (int i = 0; i < num_constraints; ++i) {
    *C_hat -= (*y_hat)(i) * this->A()[i];
  }
}

void SdpaFreeFormat::RemoveFreeVariableByTwoSlackVariablesApproach(
    std::vector<internal::BlockInX>* X_hat_blocks,
    std::vector<Eigen::SparseMatrix<double>>* A_hat,
    Eigen::SparseMatrix<double>* C_hat) const {
  *X_hat_blocks = this->X_blocks();
  X_hat_blocks->emplace_back(internal::BlockType::kDiagonal,
                             2 * this->num_free_variables());
  const int num_X_hat_rows =
      this->num_X_rows() + 2 * this->num_free_variables();
  std::vector<std::vector<Eigen::Triplet<double>>> A_hat_triplets =
      this->A_triplets();
  for (int j = 0; j < this->num_free_variables(); ++j) {
    for (Eigen::SparseMatrix<double>::InnerIterator it(this->B(), j); it;
         ++it) {
      const int i = it.row();
      // Add the entry in Âᵢ that multiplies with sⱼ.
      A_hat_triplets[i].emplace_back(this->num_X_rows() + j,
                                     this->num_X_rows() + j, it.value());
      A_hat_triplets[i].emplace_back(
          this->num_X_rows() + this->num_free_variables() + j,
          this->num_X_rows() + this->num_free_variables() + j, -it.value());
    }
  }
  A_hat->clear();
  A_hat->reserve(this->A().size());
  for (int i = 0; i < static_cast<int>(this->A().size()); ++i) {
    A_hat->emplace_back(num_X_hat_rows, num_X_hat_rows);
    A_hat->back().setFromTriplets(A_hat_triplets[i].begin(),
                                  A_hat_triplets[i].end());
  }
  // Add the entry in Ĉ that multiplies with sᵢ
  std::vector<Eigen::Triplet<double>> C_hat_triplets = this->C_triplets();
  for (Eigen::SparseMatrix<double>::InnerIterator it(this->d(), 0); it; ++it) {
    const int i = it.row();
    C_hat_triplets.emplace_back(this->num_X_rows() + i, this->num_X_rows() + i,
                                it.value());
    C_hat_triplets.emplace_back(
        this->num_X_rows() + this->num_free_variables() + i,
        this->num_X_rows() + this->num_free_variables() + i, -it.value());
  }
  *C_hat = Eigen::SparseMatrix<double>(num_X_hat_rows, num_X_hat_rows);
  C_hat->setFromTriplets(C_hat_triplets.begin(), C_hat_triplets.end());
}

void SdpaFreeFormat::RemoveFreeVariableByLorentzConeSlackApproach(
    std::vector<internal::BlockInX>* X_hat_blocks,
    std::vector<Eigen::SparseMatrix<double>>* A_hat, Eigen::VectorXd* rhs_hat,
    Eigen::SparseMatrix<double>* C_hat) const {
  *X_hat_blocks = this->X_blocks();
  X_hat_blocks->emplace_back(internal::BlockType::kMatrix,
                             this->num_free_variables() + 1);
  const int num_X_hat_rows =
      this->num_X_rows() + this->num_free_variables() + 1;
  std::vector<std::vector<Eigen::Triplet<double>>> A_hat_triplets =
      this->A_triplets();
  // Âᵢ = diag(Aᵢ, B̂ᵢ)
  // where B̂ᵢ = ⎡0  bᵢᵀ/2⎤
  //            ⎣bᵢ/2   0⎦
  for (int j = 0; j < this->num_free_variables(); ++j) {
    for (Eigen::SparseMatrix<double>::InnerIterator it(this->B(), j); it;
         ++it) {
      const int i = it.row();
      // Add the entry in Âᵢ that multiplies with sⱼ.
      A_hat_triplets[i].emplace_back(
          this->num_X_rows(), this->num_X_rows() + j + 1, it.value() / 2);
      A_hat_triplets[i].emplace_back(this->num_X_rows() + j + 1,
                                     this->num_X_rows(), it.value() / 2);
    }
  }
  const int num_linear_eq_constr =
      this->A().size() + (num_free_variables_ * (num_free_variables_ + 1)) / 2;
  A_hat->clear();
  A_hat->reserve(num_linear_eq_constr);
  for (int i = 0; i < static_cast<int>(this->A().size()); ++i) {
    A_hat->emplace_back(num_X_hat_rows, num_X_hat_rows);
    A_hat->back().setFromTriplets(A_hat_triplets[i].begin(),
                                  A_hat_triplets[i].end());
  }
  // The new right hand side is the same as the old one, except padding 0's
  // in the end.
  *rhs_hat = Eigen::VectorXd::Zero(num_linear_eq_constr);
  rhs_hat->head(this->A().size()) = this->g();

  for (int i = 0; i < num_free_variables_; ++i) {
    // Append the constraint Y(i + 1, i + 1) = Y(0, 0).
    std::vector<Eigen::Triplet<double>> triplets;
    triplets.reserve(2);
    triplets.emplace_back(num_X_rows_, num_X_rows_, -1);
    triplets.emplace_back(num_X_rows_ + i + 1, num_X_rows_ + i + 1, 1);
    A_hat->emplace_back(num_X_hat_rows, num_X_hat_rows);
    A_hat->back().setFromTriplets(triplets.begin(), triplets.end());
    for (int j = i + 1; j < num_free_variables_; ++j) {
      // Append the constraint Y(i+1, j+1) = 0
      triplets.clear();
      triplets.reserve(2);
      triplets.emplace_back(num_X_rows_ + i + 1, num_X_rows_ + j + 1, 0.5);
      triplets.emplace_back(num_X_rows_ + j + 1, num_X_rows_ + i + 1, 0.5);
      A_hat->emplace_back(num_X_hat_rows, num_X_hat_rows);
      A_hat->back().setFromTriplets(triplets.begin(), triplets.end());
    }
  }
  // Add the entry in Ĉ that multiplies with sᵢ.
  std::vector<Eigen::Triplet<double>> C_hat_triplets = this->C_triplets();
  for (Eigen::SparseMatrix<double>::InnerIterator it(this->d(), 0); it; ++it) {
    const int i = it.row();
    C_hat_triplets.emplace_back(this->num_X_rows(), this->num_X_rows() + i + 1,
                                it.value() / 2);
    C_hat_triplets.emplace_back(this->num_X_rows() + i + 1, this->num_X_rows(),
                                it.value() / 2);
  }
  *C_hat = Eigen::SparseMatrix<double>(num_X_hat_rows, num_X_hat_rows);
  C_hat->setFromTriplets(C_hat_triplets.begin(), C_hat_triplets.end());
}

namespace {
bool GenerateSdpaImpl(const std::vector<BlockInX>& X_blocks,
                      const Eigen::SparseMatrix<double>& C,
                      const std::vector<Eigen::SparseMatrix<double>>& A,
                      const Eigen::Ref<const Eigen::VectorXd>& g,
                      const std::string& file_name) {
  const int num_X_rows = C.rows();
  DRAKE_DEMAND(C.cols() == num_X_rows);
  std::ofstream sdpa_file;
  sdpa_file.open(file_name + ".dat-s", std::ios::out | std::ios::trunc);
  if (sdpa_file.is_open()) {
    // First line, number of constraints.
    sdpa_file << g.rows() << "\n";
    // Second line, number of blocks in X.
    sdpa_file << X_blocks.size() << "\n";
    // Third line, size of each block.
    for (const auto& X_block : X_blocks) {
      switch (X_block.block_type) {
        case internal::BlockType::kMatrix: {
          sdpa_file << X_block.num_rows;
          break;
        }
        case internal::BlockType::kDiagonal: {
          // Negative value indates diagonal block according to SDPA format.
          sdpa_file << -X_block.num_rows;
          break;
        }
      }
      sdpa_file << " ";
    }
    sdpa_file << "\n";
    // Forth line, the right-hand side of the constraint g.
    std::stringstream g_stream;
    g_stream << std::setprecision(20);
    g_stream << g.transpose() << "\n";
    sdpa_file << g_stream.str();
    // block_start_rows[i] records the starting row index of the i'th block in
    // X. row_to_block_indices[i] records the index of the block that X(i, i)
    // belongs to.
    std::vector<int> block_start_rows(X_blocks.size());
    std::vector<int> row_to_block_indices(num_X_rows);
    int X_row_count = 0;
    for (int i = 0; i < static_cast<int>(X_blocks.size()); ++i) {
      block_start_rows[i] = X_row_count;
      for (int j = X_row_count; j < X_row_count + X_blocks[i].num_rows; ++j) {
        row_to_block_indices[j] = i;
      }
      X_row_count += X_blocks[i].num_rows;
    }
    // The non-zero entries in C
    for (int i = 0; i < num_X_rows; ++i) {
      for (Eigen::SparseMatrix<double>::InnerIterator it(C, i); it; ++it) {
        if (it.row() <= it.col()) {
          const int block_start_row = block_start_rows[row_to_block_indices[i]];
          sdpa_file << 0 /* 0 for cost matrix C */ << " "
                    << row_to_block_indices[i] +
                           1 /* block number, starts from 1 */
                    << " "
                    << it.row() - block_start_row +
                           1 /* block row index, starts from 1*/
                    << " "
                    << i - block_start_row +
                           1 /* block column index, starts from 1*/
                    << " " << std::setprecision(20) << it.value() << "\n";
        }
      }
    }
    // The remaining lines are for A
    for (int i = 0; i < static_cast<int>(A.size()); ++i) {
      for (int j = 0; j < num_X_rows; ++j) {
        for (Eigen::SparseMatrix<double>::InnerIterator it(A[i], j); it; ++it) {
          if (it.row() <= it.col()) {
            const int block_start_row =
                block_start_rows[row_to_block_indices[j]];
            sdpa_file << i + 1 /* constraint number, starts from 1 */ << " "
                      << row_to_block_indices[j] +
                             1 /* block number, starts from 1 */
                      << " " << it.row() - block_start_row + 1 << " "
                      << j - block_start_row + 1 << std::setprecision(20) << " "
                      << it.value() << "\n";
          }
        }
      }
    }

  } else {
    std::cout << "GenerateSDPA(): Cannot open the file " << file_name
              << ".dat-s\n";
    return false;
  }
  sdpa_file.close();
  return true;
}
}  // namespace

}  // namespace internal

bool GenerateSDPA(const MathematicalProgram& prog, const std::string& file_name,
                  RemoveFreeVariableMethod method) {
  const internal::SdpaFreeFormat sdpa_free_format(prog);
  if (sdpa_free_format.num_free_variables() == 0) {
    return internal::GenerateSdpaImpl(
        sdpa_free_format.X_blocks(), sdpa_free_format.C(), sdpa_free_format.A(),
        sdpa_free_format.g(), file_name);
  }
  switch (method) {
    case RemoveFreeVariableMethod::kNullspace: {
      Eigen::SparseMatrix<double> C_hat;
      std::vector<Eigen::SparseMatrix<double>> A_hat;
      Eigen::VectorXd rhs_hat, y_hat;
      Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>>
          QR_B;
      sdpa_free_format.RemoveFreeVariableByNullspaceApproach(
          &C_hat, &A_hat, &rhs_hat, &y_hat, &QR_B);
      return internal::GenerateSdpaImpl(sdpa_free_format.X_blocks(), C_hat,
                                        A_hat, rhs_hat, file_name);
      break;
    }
    case RemoveFreeVariableMethod::kTwoSlackVariables: {
      std::vector<internal::BlockInX> X_hat_blocks;
      std::vector<Eigen::SparseMatrix<double>> A_hat;
      Eigen::SparseMatrix<double> C_hat;
      sdpa_free_format.RemoveFreeVariableByTwoSlackVariablesApproach(
          &X_hat_blocks, &A_hat, &C_hat);
      return internal::GenerateSdpaImpl(X_hat_blocks, C_hat, A_hat,
                                        sdpa_free_format.g(), file_name);
    }
    case RemoveFreeVariableMethod::kLorentzConeSlack: {
      std::vector<internal::BlockInX> X_hat_blocks;
      std::vector<Eigen::SparseMatrix<double>> A_hat;
      Eigen::VectorXd rhs_hat;
      Eigen::SparseMatrix<double> C_hat;
      sdpa_free_format.RemoveFreeVariableByLorentzConeSlackApproach(
          &X_hat_blocks, &A_hat, &rhs_hat, &C_hat);
      return internal::GenerateSdpaImpl(X_hat_blocks, C_hat, A_hat, rhs_hat,
                                        file_name);
    }
  }
  return false;
}
}  // namespace solvers
}  // namespace drake