text: // Ceres Solver - A fast non-linear least squares minimizer
  // Copyright 2019 Google Inc. All rights reserved.
  // http://code.google.com/p/ceres-solver/
  //
  // 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: darius.rueckert@fau.de (Darius Rueckert)
  
  #include "AutodiffCodegen/internal/expression_graph.h"
  
  #include "glog/logging.h"
  namespace ceres {
  namespace internal {
  
  static ExpressionGraph* expression_pool = nullptr;
  
  void StartRecordingExpressions() {
    CHECK(expression_pool == nullptr)
        << "Expression recording must be stopped before calling "
           "StartRecordingExpressions again.";
    expression_pool = new ExpressionGraph;
  }
  
  ExpressionGraph StopRecordingExpressions() {
    CHECK(expression_pool)
        << "Expression recording hasn't started yet or you tried "
           "to stop it twice.";
    ExpressionGraph result = std::move(*expression_pool);
    delete expression_pool;
    expression_pool = nullptr;
    return result;
  }
  
  ExpressionGraph* GetCurrentExpressionGraph() { return expression_pool; }
  
  bool ExpressionGraph::DependsOn(ExpressionId A, ExpressionId B) const {
    // Depth first search on the expression graph
    // Equivalent Recursive Implementation:
    //   if (A.DirectlyDependsOn(B)) return true;
    //   for (auto p : A.params_) {
    //     if (pool[p.id].DependsOn(B, pool)) return true;
    //   }
    std::vector<ExpressionId> stack = ExpressionForId(A).arguments();
    while (!stack.empty()) {
      auto top = stack.back();
      stack.pop_back();
      if (top == B) {
        return true;
      }
      auto& expr = ExpressionForId(top);
      stack.insert(stack.end(), expr.arguments().begin(), expr.arguments().end());
    }
    return false;
  }
  
  bool ExpressionGraph::operator==(const ExpressionGraph& other) const {
    if (Size() != other.Size()) {
      return false;
    }
    for (ExpressionId id = 0; id < Size(); ++id) {
      if (!(ExpressionForId(id) == other.ExpressionForId(id))) {
        return false;
      }
    }
    return true;
  }
  
  void ExpressionGraph::Erase(ExpressionId location) {
    CHECK_GE(location, 0);
    CHECK_LT(location, Size());
    // Move everything after id to the front and update references
    for (ExpressionId id = location + 1; id < Size(); ++id) {
      expressions_[id - 1] = expressions_[id];
      auto& expression = expressions_[id - 1];
      // Decrement reference if it points to a shifted variable.
      if (expression.lhs_id() >= location) {
        expression.set_lhs_id(expression.lhs_id() - 1);
      }
      for (auto& arg : *expression.mutable_arguments()) {
        if (arg >= location) {
          arg--;
        }
      }
    }
    expressions_.resize(Size() - 1);
  }
  
  void ExpressionGraph::Insert(ExpressionId location,
                               const Expression& expression) {
    CHECK_GE(location, 0);
    CHECK_LE(location, Size());
    ExpressionId last_expression_id = Size() - 1;
    // Increase size by adding a dummy expression.
    expressions_.push_back(Expression());
  
    // Move everything after id back and update references
    for (ExpressionId id = last_expression_id; id >= location; --id) {
      auto& expression = expressions_[id];
      // Increment reference if it points to a shifted variable.
      if (expression.lhs_id() >= location) {
        expression.set_lhs_id(expression.lhs_id() + 1);
      }
      for (auto& arg : *expression.mutable_arguments()) {
        if (arg >= location) {
          arg++;
        }
      }
      expressions_[id + 1] = expression;
    }
  
    if (expression.IsControlExpression() ||
        expression.lhs_id() != kInvalidExpressionId) {
      // Insert new expression at the correct place
      expressions_[location] = expression;
    } else {
      // Arithmetic expression with invalid lhs
      // -> Set lhs to location
      Expression copy = expression;
      copy.set_lhs_id(location);
      expressions_[location] = copy;
    }
  }
  
  ExpressionId ExpressionGraph::InsertBack(const Expression& expression) {
    if (expression.IsControlExpression()) {
      // Control expression are just added to the list. We do not return a
      // reference to them.
      CHECK(expression.lhs_id() == kInvalidExpressionId)
          << "Control expressions must have an invalid lhs.";
      expressions_.push_back(expression);
      return kInvalidExpressionId;
    }
  
    if (expression.lhs_id() == kInvalidExpressionId) {
      // Create a new variable name for this expression and set it as the lhs
      Expression copy = expression;
      copy.set_lhs_id(static_cast<ExpressionId>(expressions_.size()));
      expressions_.push_back(copy);
    } else {
      // The expressions writes to a variable declared in the past
      // -> Just add it to the list
      CHECK_LE(expression.lhs_id(), expressions_.size())
          << "The left hand side must reference a variable in the past.";
      expressions_.push_back(expression);
    }
  
    return Size() - 1;
  }
  
  ExpressionId ExpressionGraph::FindMatchingEndif(ExpressionId id) const {
    CHECK(ExpressionForId(id).type() == ExpressionType::IF)
        << "FindClosingControlExpression is only valid on IF "
           "expressions.";
  
    // Traverse downwards
    for (ExpressionId i = id + 1; i < Size(); ++i) {
      const auto& expr = ExpressionForId(i);
      if (expr.type() == ExpressionType::ENDIF) {
        return i;
  
      } else if (expr.type() == ExpressionType::IF) {
        // Found a nested IF.
        // -> Jump over the block and continue behind it.
        auto matching_endif = FindMatchingEndif(i);
        if (matching_endif == kInvalidExpressionId) {
          return kInvalidExpressionId;
        }
        i = matching_endif;
        continue;
      }
    }
    return kInvalidExpressionId;
  }
  
  ExpressionId ExpressionGraph::FindMatchingElse(ExpressionId id) const {
    CHECK(ExpressionForId(id).type() == ExpressionType::IF)
        << "FindClosingControlExpression is only valid on IF "
           "expressions.";
  
    // Traverse downwards
    for (ExpressionId i = id + 1; i < Size(); ++i) {
      const auto& expr = ExpressionForId(i);
      if (expr.type() == ExpressionType::ELSE) {
        // Found it!
        return i;
      } else if (expr.type() == ExpressionType::ENDIF) {
        // Found an endif even though we were looking for an ELSE.
        // -> Return invalidId
        return kInvalidExpressionId;
      } else if (expr.type() == ExpressionType::IF) {
        // Found a nested IF.
        // -> Jump over the block and continue behind it.
        auto matching_endif = FindMatchingEndif(i);
        if (matching_endif == kInvalidExpressionId) {
          return kInvalidExpressionId;
        }
        i = matching_endif;
        continue;
      }
    }
    return kInvalidExpressionId;
  }
  
  }  // namespace internal
  }  // namespace ceres
  