// Ceres Solver - A fast non-linear least squares minimizer

// Copyright 2015 Google Inc. All rights reserved.

// http://ceres-solver.org/

//

// Redistribution and use in source and binary forms, with or without

// modification, are permitted provided that the following conditions are met:

//

// * Redistributions of source code must retain the above copyright notice,

//   this list of conditions and the following disclaimer.

// * Redistributions in binary form must reproduce the above copyright notice,

//   this list of conditions and the following disclaimer in the documentation

//   and/or other materials provided with the distribution.

// * Neither the name of Google Inc. nor the names of its contributors may be

//   used to endorse or promote products derived from this software without

//   specific prior written permission.

//

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"

// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE

// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE

// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR

// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF

// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS

// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN

// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)

// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

// POSSIBILITY OF SUCH DAMAGE.

//

// Author: sameeragarwal@google.com (Sameer Agarwal)

#include "ceres/linear_least_squares_problems.h"

#include <cstdio>

#include <string>

#include <vector>

#include "ceres/block_sparse_matrix.h"

#include "ceres/block_structure.h"

#include "ceres/casts.h"

#include "ceres/file.h"

#include "ceres/internal/scoped_ptr.h"

#include "ceres/stringprintf.h"

#include "ceres/triplet_sparse_matrix.h"

#include "ceres/types.h"

#include "glog/logging.h"

namespace ceres {

namespace internal {

using std::string;

LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromId(int id) {

  switch (id) {

    case 0:

      return LinearLeastSquaresProblem0();

    case 1:

      return LinearLeastSquaresProblem1();

    case 2:

      return LinearLeastSquaresProblem2();

    case 3:

      return LinearLeastSquaresProblem3();

    case 4:

      return LinearLeastSquaresProblem4();

    default:

      LOG(FATAL) << "Unknown problem id requested " << id;

  }

  return NULL;

}

/*

A = [1   2]

    [3   4]

    [6 -10]

b = [  8

      18

     -18]

x = [2

     3]

D = [1

     2]

x_D = [1.78448275;

       2.82327586;]

 */

LinearLeastSquaresProblem* LinearLeastSquaresProblem0() {

  LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem;

  TripletSparseMatrix* A = new TripletSparseMatrix(3, 2, 6);

  problem->b.reset(new double[3]);

  problem->D.reset(new double[2]);

  problem->x.reset(new double[2]);

  problem->x_D.reset(new double[2]);

  int* Ai = A->mutable_rows();

  int* Aj = A->mutable_cols();

  double* Ax = A->mutable_values();

  int counter = 0;

  for (int i = 0; i < 3; ++i) {

    for (int j = 0; j< 2; ++j) {

      Ai[counter] = i;

      Aj[counter] = j;

      ++counter;

    }

  }

  Ax[0] = 1.;

  Ax[1] = 2.;

  Ax[2] = 3.;

  Ax[3] = 4.;

  Ax[4] = 6;

  Ax[5] = -10;

  A->set_num_nonzeros(6);

  problem->A.reset(A);

  problem->b[0] = 8;

  problem->b[1] = 18;

  problem->b[2] = -18;

  problem->x[0] = 2.0;

  problem->x[1] = 3.0;

  problem->D[0] = 1;

  problem->D[1] = 2;

  problem->x_D[0] = 1.78448275;

  problem->x_D[1] = 2.82327586;

  return problem;

}

/*

      A = [1 0  | 2 0 0

           3 0  | 0 4 0

           0 5  | 0 0 6

           0 7  | 8 0 0

           0 9  | 1 0 0

           0 0  | 1 1 1]

      b = [0

           1

           2

           3

           4

           5]

      c = A'* b = [ 3

                   67

                   33

                    9

                   17]

      A'A = [10    0    2   12   0

              0  155   65    0  30

              2   65   70    1   1

             12    0    1   17   1

              0   30    1    1  37]

      S = [ 42.3419  -1.4000  -11.5806

            -1.4000   2.6000    1.0000

            11.5806   1.0000   31.1935]

      r = [ 4.3032

            5.4000

            5.0323]

      S\r = [ 0.2102

              2.1367

              0.1388]

      A\b = [-2.3061

              0.3172

              0.2102

              2.1367

              0.1388]

*/

// The following two functions create a TripletSparseMatrix and a

// BlockSparseMatrix version of this problem.

// TripletSparseMatrix version.

LinearLeastSquaresProblem* LinearLeastSquaresProblem1() {

  int num_rows = 6;

  int num_cols = 5;

  LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem;

  TripletSparseMatrix* A = new TripletSparseMatrix(num_rows,

                                                   num_cols,

                                                   num_rows * num_cols);

  problem->b.reset(new double[num_rows]);

  problem->D.reset(new double[num_cols]);

  problem->num_eliminate_blocks = 2;

  int* rows = A->mutable_rows();

  int* cols = A->mutable_cols();

  double* values = A->mutable_values();

  int nnz = 0;

  // Row 1

  {

    rows[nnz] = 0;

    cols[nnz] = 0;

    values[nnz++] = 1;

    rows[nnz] = 0;

    cols[nnz] = 2;

    values[nnz++] = 2;

  }

  // Row 2

  {

    rows[nnz] = 1;

    cols[nnz] = 0;

    values[nnz++] = 3;

    rows[nnz] = 1;

    cols[nnz] = 3;

    values[nnz++] = 4;

  }

  // Row 3

  {

    rows[nnz] = 2;

    cols[nnz] = 1;

    values[nnz++] = 5;

    rows[nnz] = 2;

    cols[nnz] = 4;

    values[nnz++] = 6;

  }

  // Row 4

  {

    rows[nnz] = 3;

    cols[nnz] = 1;

    values[nnz++] = 7;

    rows[nnz] = 3;

    cols[nnz] = 2;

    values[nnz++] = 8;

  }

  // Row 5

  {

    rows[nnz] = 4;

    cols[nnz] = 1;

    values[nnz++] = 9;

    rows[nnz] = 4;

    cols[nnz] = 2;

    values[nnz++] = 1;

  }

  // Row 6

  {

    rows[nnz] = 5;

    cols[nnz] = 2;

    values[nnz++] = 1;

    rows[nnz] = 5;

    cols[nnz] = 3;

    values[nnz++] = 1;

    rows[nnz] = 5;

    cols[nnz] = 4;

    values[nnz++] = 1;

  }

  A->set_num_nonzeros(nnz);

  CHECK(A->IsValid());

  problem->A.reset(A);

  for (int i = 0; i < num_cols; ++i) {

    problem->D.get()[i] = 1;

  }

  for (int i = 0; i < num_rows; ++i) {

    problem->b.get()[i] = i;

  }

  return problem;

}

// BlockSparseMatrix version

LinearLeastSquaresProblem* LinearLeastSquaresProblem2() {

  int num_rows = 6;

  int num_cols = 5;

  LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem;

  problem->b.reset(new double[num_rows]);

  problem->D.reset(new double[num_cols]);

  problem->num_eliminate_blocks = 2;

  CompressedRowBlockStructure* bs = new CompressedRowBlockStructure;

  scoped_array<double> values(new double[num_rows * num_cols]);

  for (int c = 0; c < num_cols; ++c) {

    bs->cols.push_back(Block());

    bs->cols.back().size = 1;

    bs->cols.back().position = c;

  }

  int nnz = 0;

  // Row 1

  {

    values[nnz++] = 1;

    values[nnz++] = 2;

    bs->rows.push_back(CompressedRow());

    CompressedRow& row = bs->rows.back();

    row.block.size = 1;

    row.block.position = 0;

    row.cells.push_back(Cell(0, 0));

    row.cells.push_back(Cell(2, 1));

  }

  // Row 2

  {

    values[nnz++] = 3;

    values[nnz++] = 4;

    bs->rows.push_back(CompressedRow());

    CompressedRow& row = bs->rows.back();

    row.block.size = 1;

    row.block.position = 1;

    row.cells.push_back(Cell(0, 2));

    row.cells.push_back(Cell(3, 3));

  }

  // Row 3

  {

    values[nnz++] = 5;

    values[nnz++] = 6;

    bs->rows.push_back(CompressedRow());

    CompressedRow& row = bs->rows.back();

    row.block.size = 1;

    row.block.position = 2;

    row.cells.push_back(Cell(1, 4));

    row.cells.push_back(Cell(4, 5));

  }

  // Row 4

  {

    values[nnz++] = 7;

    values[nnz++] = 8;

    bs->rows.push_back(CompressedRow());

    CompressedRow& row = bs->rows.back();

    row.block.size = 1;

    row.block.position = 3;

    row.cells.push_back(Cell(1, 6));

    row.cells.push_back(Cell(2, 7));

  }

  // Row 5

  {

    values[nnz++] = 9;

    values[nnz++] = 1;

    bs->rows.push_back(CompressedRow());

    CompressedRow& row = bs->rows.back();

    row.block.size = 1;

    row.block.position = 4;

    row.cells.push_back(Cell(1, 8));

    row.cells.push_back(Cell(2, 9));

  }

  // Row 6

  {

    values[nnz++] = 1;

    values[nnz++] = 1;

    values[nnz++] = 1;

    bs->rows.push_back(CompressedRow());

    CompressedRow& row = bs->rows.back();

    row.block.size = 1;

    row.block.position = 5;

    row.cells.push_back(Cell(2, 10));

    row.cells.push_back(Cell(3, 11));

    row.cells.push_back(Cell(4, 12));

  }

  BlockSparseMatrix* A = new BlockSparseMatrix(bs);

  memcpy(A->mutable_values(), values.get(), nnz * sizeof(*A->values()));

  for (int i = 0; i < num_cols; ++i) {

    problem->D.get()[i] = 1;

  }

  for (int i = 0; i < num_rows; ++i) {

    problem->b.get()[i] = i;

  }

  problem->A.reset(A);

  return problem;

}

/*

      A = [1 0

           3 0

           0 5

           0 7

           0 9

           0 0]

      b = [0

           1

           2

           3

           4

           5]

*/

// BlockSparseMatrix version

LinearLeastSquaresProblem* LinearLeastSquaresProblem3() {

  int num_rows = 5;

  int num_cols = 2;

  LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem;

  problem->b.reset(new double[num_rows]);

  problem->D.reset(new double[num_cols]);

  problem->num_eliminate_blocks = 2;

  CompressedRowBlockStructure* bs = new CompressedRowBlockStructure;

  scoped_array<double> values(new double[num_rows * num_cols]);

  for (int c = 0; c < num_cols; ++c) {

    bs->cols.push_back(Block());

    bs->cols.back().size = 1;

    bs->cols.back().position = c;

  }

  int nnz = 0;

  // Row 1

  {

    values[nnz++] = 1;

    bs->rows.push_back(CompressedRow());

    CompressedRow& row = bs->rows.back();

    row.block.size = 1;

    row.block.position = 0;

    row.cells.push_back(Cell(0, 0));

  }

  // Row 2

  {

    values[nnz++] = 3;

    bs->rows.push_back(CompressedRow());

    CompressedRow& row = bs->rows.back();

    row.block.size = 1;

    row.block.position = 1;

    row.cells.push_back(Cell(0, 1));

  }

  // Row 3

  {

    values[nnz++] = 5;

    bs->rows.push_back(CompressedRow());

    CompressedRow& row = bs->rows.back();

    row.block.size = 1;

    row.block.position = 2;

    row.cells.push_back(Cell(1, 2));

  }

  // Row 4

  {

    values[nnz++] = 7;

    bs->rows.push_back(CompressedRow());

    CompressedRow& row = bs->rows.back();

    row.block.size = 1;

    row.block.position = 3;

    row.cells.push_back(Cell(1, 3));

  }

  // Row 5

  {

    values[nnz++] = 9;

    bs->rows.push_back(CompressedRow());

    CompressedRow& row = bs->rows.back();

    row.block.size = 1;

    row.block.position = 4;

    row.cells.push_back(Cell(1, 4));

  }

  BlockSparseMatrix* A = new BlockSparseMatrix(bs);

  memcpy(A->mutable_values(), values.get(), nnz * sizeof(*A->values()));

  for (int i = 0; i < num_cols; ++i) {

    problem->D.get()[i] = 1;

  }

  for (int i = 0; i < num_rows; ++i) {

    problem->b.get()[i] = i;

  }

  problem->A.reset(A);

  return problem;

}

/*

      A = [1 2 0 0 0 1 1

           1 4 0 0 0 5 6

           0 0 9 0 0 3 1]

      b = [0

           1

           2]

*/

// BlockSparseMatrix version

//

// This problem has the unique property that it has two different

// sized f-blocks, but only one of them occurs in the rows involving

// the one e-block. So performing Schur elimination on this problem

// tests the Schur Eliminator's ability to handle non-e-block rows

// correctly when their structure does not conform to the static

// structure determined by DetectStructure.

//

// NOTE: This problem is too small and rank deficient to be solved without

// the diagonal regularization.

LinearLeastSquaresProblem* LinearLeastSquaresProblem4() {

  int num_rows = 3;

  int num_cols = 7;

  LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem;

  problem->b.reset(new double[num_rows]);

  problem->D.reset(new double[num_cols]);

  problem->num_eliminate_blocks = 1;

  CompressedRowBlockStructure* bs = new CompressedRowBlockStructure;

  scoped_array<double> values(new double[num_rows * num_cols]);

  // Column block structure

  bs->cols.push_back(Block());

  bs->cols.back().size = 2;

  bs->cols.back().position = 0;

  bs->cols.push_back(Block());

  bs->cols.back().size = 3;

  bs->cols.back().position = 2;

  bs->cols.push_back(Block());

  bs->cols.back().size = 2;

  bs->cols.back().position = 5;

  int nnz = 0;

  // Row 1 & 2

  {

    bs->rows.push_back(CompressedRow());

    CompressedRow& row = bs->rows.back();

    row.block.size = 2;

    row.block.position = 0;

    row.cells.push_back(Cell(0, nnz));

    values[nnz++] = 1;

    values[nnz++] = 2;

    values[nnz++] = 1;

    values[nnz++] = 4;

    row.cells.push_back(Cell(2, nnz));

    values[nnz++] = 1;

    values[nnz++] = 1;

    values[nnz++] = 5;

    values[nnz++] = 6;

  }

  // Row 3

  {

    bs->rows.push_back(CompressedRow());

    CompressedRow& row = bs->rows.back();

    row.block.size = 1;

    row.block.position = 2;

    row.cells.push_back(Cell(1, nnz));

    values[nnz++] = 9;

    values[nnz++] = 0;

    values[nnz++] = 0;

    row.cells.push_back(Cell(2, nnz));

    values[nnz++] = 3;

    values[nnz++] = 1;

  }

  BlockSparseMatrix* A = new BlockSparseMatrix(bs);

  memcpy(A->mutable_values(), values.get(), nnz * sizeof(*A->values()));

  for (int i = 0; i < num_cols; ++i) {

    problem->D.get()[i] = (i + 1) * 100;

  }

  for (int i = 0; i < num_rows; ++i) {

    problem->b.get()[i] = i;

  }

  problem->A.reset(A);

  return problem;

}

namespace {

bool DumpLinearLeastSquaresProblemToConsole(const SparseMatrix* A,

                                            const double* D,

                                            const double* b,

                                            const double* x,

                                            int num_eliminate_blocks) {

  CHECK_NOTNULL(A);

  Matrix AA;

  A->ToDenseMatrix(&AA;;

  LOG(INFO) << "A^T: \n" << AA.transpose();

  if (D != NULL) {

    LOG(INFO) << "A's appended diagonal:\n"

              << ConstVectorRef(D, A->num_cols());

  }

  if (b != NULL) {

    LOG(INFO) << "b: \n" << ConstVectorRef(b, A->num_rows());

  }

  if (x != NULL) {

    LOG(INFO) << "x: \n" << ConstVectorRef(x, A->num_cols());

  }

  return true;

}

void WriteArrayToFileOrDie(const string& filename,

                           const double* x,

                           const int size) {

  CHECK_NOTNULL(x);

  VLOG(2) << "Writing array to: " << filename;

  FILE* fptr = fopen(filename.c_str(), "w");

  CHECK_NOTNULL(fptr);

  for (int i = 0; i < size; ++i) {

    fprintf(fptr, "%17f\n", x[i]);

  }

  fclose(fptr);

}

bool DumpLinearLeastSquaresProblemToTextFile(const string& filename_base,

                                             const SparseMatrix* A,

                                             const double* D,

                                             const double* b,

                                             const double* x,

                                             int num_eliminate_blocks) {

  CHECK_NOTNULL(A);

  LOG(INFO) << "writing to: " << filename_base << "*";

  string matlab_script;

  StringAppendF(&matlab_script,

                "function lsqp = load_trust_region_problem()\n");

  StringAppendF(&matlab_script,

                "lsqp.num_rows = %d;\n", A->num_rows());

  StringAppendF(&matlab_script,

                "lsqp.num_cols = %d;\n", A->num_cols());

  {

    string filename = filename_base + "_A.txt";

    FILE* fptr = fopen(filename.c_str(), "w");

    CHECK_NOTNULL(fptr);

    A->ToTextFile(fptr);

    fclose(fptr);

    StringAppendF(&matlab_script,

                  "tmp = load('%s', '-ascii');\n", filename.c_str());

    StringAppendF(

        &matlab_script,

        "lsqp.A = sparse(tmp(:, 1) + 1, tmp(:, 2) + 1, tmp(:, 3), %d, %d);\n",

        A->num_rows(),

        A->num_cols());

  }

  if (D != NULL) {

    string filename = filename_base + "_D.txt";

    WriteArrayToFileOrDie(filename, D, A->num_cols());

    StringAppendF(&matlab_script,

                  "lsqp.D = load('%s', '-ascii');\n", filename.c_str());

  }

  if (b != NULL) {

    string filename = filename_base + "_b.txt";

    WriteArrayToFileOrDie(filename, b, A->num_rows());

    StringAppendF(&matlab_script,

                  "lsqp.b = load('%s', '-ascii');\n", filename.c_str());

  }

  if (x != NULL) {

    string filename = filename_base + "_x.txt";

    WriteArrayToFileOrDie(filename, x, A->num_cols());

    StringAppendF(&matlab_script,

                  "lsqp.x = load('%s', '-ascii');\n", filename.c_str());

  }

  string matlab_filename = filename_base + ".m";

  WriteStringToFileOrDie(matlab_script, matlab_filename);

  return true;

}

}  // namespace

bool DumpLinearLeastSquaresProblem(const string& filename_base,

                                   DumpFormatType dump_format_type,

                                   const SparseMatrix* A,

                                   const double* D,

                                   const double* b,

                                   const double* x,

                                   int num_eliminate_blocks) {

  switch (dump_format_type) {

    case CONSOLE:

      return DumpLinearLeastSquaresProblemToConsole(A, D, b, x,

                                                    num_eliminate_blocks);

    case TEXTFILE:

      return DumpLinearLeastSquaresProblemToTextFile(filename_base,

                                                     A, D, b, x,

                                                     num_eliminate_blocks);

    default:

      LOG(FATAL) << "Unknown DumpFormatType " << dump_format_type;

  }

  return true;

}

}  // namespace internal

}  // namespace ceres