switch (E->getSelectKind()) {
  default:
    break;
  case CMSelectExpr::SK_select:
    return EmitSelect(CGF, E, BaseAddr);
  case CMSelectExpr::SK_row:
  case CMSelectExpr::SK_column:
    return EmitRowColumnSelect(CGF, E, BaseAddr);
  case CMSelectExpr::SK_element:
    return EmitElementSelect(CGF, E, BaseAddr);
  case CMSelectExpr::SK_subscript:
    return EmitSubscriptSelect(CGF, E, BaseAddr);
  case CMSelectExpr::SK_select_all:
    return EmitSelectAll(CGF, E, BaseAddr);
  }

/*
 *
 */

// CM specific code generation.

#include "CGCM.h"
#include "CodeGenModule.h"
#include "clang/AST/DeclTemplate.h"
#include "clang/AST/Expr.h"
#include "clang/AST/ExprCM.h"
#include "llvm/IR/Intrinsics.h"
#include "llvm/Support/Path.h"

using namespace clang;
using namespace CodeGen;

CGCMRegionInfo::CGCMRegionInfo(RegionKind Kind, LValue Base, unsigned vSize,
                               unsigned vStride, unsigned hSize,
                               unsigned hStride, llvm::Value *vOffset,
                               llvm::Value *hOffset)
    : Kind(Kind), Base(Base) {
  setVSize(vSize);
  setVStride(vStride);
  setHSize(hSize);
  setHStride(hStride);
  setVOffset(vOffset);
  setHOffset(hOffset);
}

CGCMRegionInfo::CGCMRegionInfo(RegionKind Kind, LValue Base, unsigned Size,
                               unsigned Stride, llvm::Value *Offset)
    : Kind(Kind), Base(Base) {
  setSize(Size);
  setStride(Stride);
  setVOffset(Offset);

  // fill zeros for other fields.
  Dims[2] = Dims[3] = 0;
  Indices[1] = 0;
}

CGCMRegionInfo::CGCMRegionInfo(RegionKind Kind, LValue Base)
    : Kind(Kind), Base(Base) {
  // fill zeros for other fields.
  Dims[0] = Dims[1] = Dims[2] = Dims[3] = 0;
  Indices[0] = Indices[1] = 0;
}

unsigned CGCMRegionInfo::getBaseWidth() const {
  assert(is2DRegion());

  QualType Ty = Base.getType();
  assert(Ty->isCMMatrixType() && "matrix type expected");
  return Ty->getAs<CMMatrixType>()->getNumColumns();
}

unsigned CGCMRegionInfo::getBaseHeight() const {
  assert(is2DRegion());

  QualType Ty = Base.getType();
  assert(Ty->isCMMatrixType() && "matrix type expected");
  return Ty->getAs<CMMatrixType>()->getNumRows();
}

unsigned CGCMRegionInfo::getBaseSize() const {
  assert(is1DRegion());

  QualType Ty = Base.getType();
  assert(Ty->isCMVectorType() && "vector type expected");
  return Ty->getAs<CMVectorType>()->getNumElements();
}

void CGCMRegionInfo::print(raw_ostream &OS) const {
  if (isFormat()) {
    OS << "{ format, " << Base.getType().getAsString() << " }";
    return;
  }

  OS << "{ ";
  if (Base.isSimple())
    OS << Base.getAddress().getPointer()  << "(base)";
  else if (Base.isCMRegion())
    OS << Base.getCMRegionAddr() << "(nested)";

  // Print all fields as it is.
  for (unsigned i = 0; i < 4; ++i)
    OS << ", " << Dims[i];

  OS << "; ";

  // Print all indices
  for (unsigned i = 0; i < 2; ++i)
    if (Indices[i] != 0) {
      Indices[i]->print(OS);
      OS << ", ";
    }

  OS << "}";
}

void CGCMRegionInfo::dump() const { print(llvm::errs()); }

// CM specific expressions codegen.

static unsigned EmitAsConstantInt(CodeGenFunction &CGF, const Expr *E) {
  llvm::Value *Val = CGF.EmitAnyExpr(E).getScalarVal();
  assert(isa<llvm::ConstantInt>(Val) && "not a constant integer");
  return static_cast<unsigned>(cast<llvm::ConstantInt>(Val)->getZExtValue());
}

static llvm::Value *getBaseAddr(LValue Base) {
  if (Base.isSimple())
    return Base.getAddress().getPointer();
  else if (Base.isCMRegion())
    return Base.getCMRegionAddr();

  llvm_unreachable("unexpected base lvalue");
}

LValue CGCMRuntime::EmitSelect(CodeGenFunction &CGF, const CMSelectExpr *E,
                               LValue Base) {
  assert(E->isSelect());
  if (E->is2D()) {
    // Emit vSize, vStride, hSize, hStride
    unsigned vSize = EmitAsConstantInt(CGF, E->getVSize());
    unsigned vStride = EmitAsConstantInt(CGF, E->getVStride());
    unsigned hSize = EmitAsConstantInt(CGF, E->getHSize());
    unsigned hStride = EmitAsConstantInt(CGF, E->getHStride());

    // Emit vOffset
    llvm::Value *vOffset = CGF.EmitAnyExpr(E->getVOffset()).getScalarVal();
    assert(vOffset && vOffset->getType()->isIntegerTy());

    // Emit hOffset
    llvm::Value *hOffset = CGF.EmitAnyExpr(E->getHOffset()).getScalarVal();
    assert(hOffset && hOffset->getType()->isIntegerTy());

    const CGCMRegionInfo &RI = addRegionInfo(
        new CGCMRegionInfo(CGCMRegionInfo::RK_select, Base, vSize, vStride,
                           hSize, hStride, vOffset, hOffset));

    return LValue::MakeCMRegion(getBaseAddr(Base), RI, E->getType(),
                                Base.getAlignment());
  }

  assert(E->is1D());
  // Emit size and stride.
  unsigned Size = EmitAsConstantInt(CGF, E->getSize());
  unsigned Stride = EmitAsConstantInt(CGF, E->getStride());

  // Emit offset.
  llvm::Value *Offset = CGF.EmitAnyExpr(E->getOffset()).getScalarVal();
  assert(Offset && Offset->getType()->isIntegerTy());

  const CGCMRegionInfo &RI = addRegionInfo(new CGCMRegionInfo(
      CGCMRegionInfo::RK_select, Base, Size, Stride, Offset));

  return LValue::MakeCMRegion(getBaseAddr(Base), RI, E->getType(),
                              Base.getAlignment());
}

LValue CGCMRuntime::EmitRowColumnSelect(CodeGenFunction &CGF,
                                        const CMSelectExpr *E, LValue Base) {
  assert(E->isColumnSelect() || E->isRowSelect());
  assert(E->is2D() && "not selecting matrix");

  const CMMatrixType *MT = E->getBase()->getType()->getAs<CMMatrixType>();
  unsigned Height = MT->getNumRows();
  unsigned Width = MT->getNumColumns();

  // Selecting a row is equivalent to 2D select with arguments:
  if (E->isRowSelect()) {
    llvm::Value *Index = CGF.EmitAnyExpr(E->getRowExpr()).getScalarVal();
    llvm::Value *Zero = llvm::Constant::getNullValue(Index->getType());
    const CGCMRegionInfo &RI = addRegionInfo(new CGCMRegionInfo(
        CGCMRegionInfo::RK_select, Base, 1, 0, Width, 1, Index, Zero));

    return LValue::MakeCMRegion(getBaseAddr(Base), RI, E->getType(),
                                Base.getAlignment());
  }

  // Selecting a column is equivalent to 2D select with arguments:
  llvm::Value *Index = CGF.EmitAnyExpr(E->getColumnExpr()).getScalarVal();
  llvm::Value *Zero = llvm::Constant::getNullValue(Index->getType());
  const CGCMRegionInfo &RI = addRegionInfo(new CGCMRegionInfo(
      CGCMRegionInfo::RK_select, Base, Height, 1, 1, 1, Zero, Index));

  return LValue::MakeCMRegion(getBaseAddr(Base), RI, E->getType(),
                              Base.getAlignment());
}

LValue CGCMRuntime::EmitElementSelect(CodeGenFunction &CGF,
                                      const CMSelectExpr *E, LValue Base) {
  assert(E->isElementSelect());

  // Selecting a single element from a vector is a regular select with:
  if (E->is1D()) {
    llvm::Value *Index = CGF.EmitAnyExpr(E->getIndex()).getScalarVal();
    const CGCMRegionInfo &RI = addRegionInfo(
        new CGCMRegionInfo(CGCMRegionInfo::RK_select, Base, 1, 0, Index));

    return LValue::MakeCMRegion(getBaseAddr(Base), RI, E->getType(),
                                Base.getAlignment());
  }

  assert(E->is2D());
  // Selecting a single element from a matrix is a regular select with:
  llvm::Value *RowIndex = CGF.EmitAnyExpr(E->getRowIndex()).getScalarVal();
  llvm::Value *ColIndex = CGF.EmitAnyExpr(E->getColumnIndex()).getScalarVal();
  const CGCMRegionInfo &RI = addRegionInfo(new CGCMRegionInfo(
      CGCMRegionInfo::RK_select, Base, 1, 0, 1, 0, RowIndex, ColIndex));

  return LValue::MakeCMRegion(getBaseAddr(Base), RI, E->getType(),
                              Base.getAlignment());
}

LValue CGCMRuntime::EmitSubscriptSelect(CodeGenFunction &CGF,
                                        const CMSelectExpr *E, LValue Base) {
  assert(E->isSubscriptSelect());

  // Selecting a single element from a vector is a regular select with:
  if (E->is1D()) {
    llvm::Value *Index = CGF.EmitAnyExpr(E->getSubscriptIndex()).getScalarVal();
    const CGCMRegionInfo &RI = addRegionInfo(
        new CGCMRegionInfo(CGCMRegionInfo::RK_select, Base, 1, 0, Index));

    return LValue::MakeCMRegion(getBaseAddr(Base), RI, E->getType(),
                                Base.getAlignment());
  }

  assert(E->is2D());
  assert(E->getType()->isCMVectorType());
  unsigned Width = E->getType()->getCMVectorMatrixSize();
  // Selecting a row from a matrix is a regular select with:
  llvm::Value *Index = CGF.EmitAnyExpr(E->getSubscriptIndex()).getScalarVal();
  llvm::Value *Zero = llvm::Constant::getNullValue(Index->getType());
  const CGCMRegionInfo &RI = addRegionInfo(new CGCMRegionInfo(
      CGCMRegionInfo::RK_select, Base, 1, 0, Width, 1, Index, Zero));

  return LValue::MakeCMRegion(getBaseAddr(Base), RI, E->getType(),
                              Base.getAlignment());
}

LValue CGCMRuntime::EmitSelectAll(CodeGenFunction &CGF, const CMSelectExpr *E,
                                  LValue Base) {
  assert(E->isSelectAll());
  // Simply returns the base.
  return Base;
}

LValue CGCMRuntime::EmitCMSelectExprLValue(CodeGenFunction &CGF,
                                           const CMSelectExpr *E) {
  const Expr *Base = E->getBase();

  // This is a bug in the old compiler's implementation:
  // CM provides a set of "select" functions for referencing a subset of the
  // elements of vector/matrix objects. All these operations (except "iselect",
  // "genx_select", and "replicate") return a reference to the elements of
  // matrix/vector objects, so they can be used as L-values in the statements.
  // This means it is illegal to select a rvalue. On the other hand, C/C++ does
  // allow member accesses of rvalues, but the resulting expressions are also
  // rvalues. We have to somehow get over this defect, i.e. create a temporal
  // object internally and use it as the base. Approximately this is applying
  // some rvalue reference semantics to vector_ref or matrix_ref (!).
  LValue BaseAddr;
  if (Base->isRValue()) {
    llvm::Value *Val = CGF.EmitAnyExpr(Base).getScalarVal();
    llvm::Value *Addr = CGF.CreateTempAlloca(Val->getType(), "rvalue.addr");
    CGF.Builder.CreateDefaultAlignedStore(Val, Addr);
    BaseAddr = CGF.MakeNaturalAlignAddrLValue(Addr, Base->getType());
  } else
    BaseAddr = CGF.EmitLValue(Base);

  llvm_unreachable("not implemented yet");
}

llvm::Value *CGCMRuntime::EmitCMSelectExpr(CodeGenFunction &CGF,
                                           const CMSelectExpr *E) {
  // A replicate select is always an RValue.
  if (E->isReplicate()) {
    assert(E->isRValue());
    return EmitReplicateSelect(CGF, E);
  }

  // An iselect is always an RValue.
  if (E->isISelect()) {
    assert(E->isRValue());
    return EmitISelect(CGF, E);
  }

  assert(E->isLValue());
  LValue LV = EmitCMSelectExprLValue(CGF, E);
  return CGF.EmitLoadOfLValue(LV, E->getExprLoc()).getScalarVal();
}

LValue CGCMRuntime::EmitCMFormatExprLValue(CodeGenFunction &CGF,
                                           const CMFormatExpr *E) {
  const Expr *Base = E->getBase();
  LValue BaseLV = CGF.EmitLValue(Base);

  const CGCMRegionInfo &RI =
      addRegionInfo(new CGCMRegionInfo(CGCMRegionInfo::RK_format, BaseLV));

  return LValue::MakeCMRegion(getBaseAddr(BaseLV), RI, E->getType(),
                              BaseLV.getAlignment());
}

llvm::Value *CGCMRuntime::EmitCMFormatExpr(CodeGenFunction &CGF,
                                           const CMFormatExpr *E) {
  if (E->isRValue()) {
    llvm::Value *Val = CGF.EmitAnyExpr(E->getBase()).getScalarVal();
    llvm::Type *Ty = CGF.ConvertType(E->getType());
    // In case the base is a reference.
    if (Ty->isPointerTy())
      Ty = Ty->getPointerElementType();
    Val = CGF.Builder.CreateBitCast(Val, Ty);
    return Val;
  }

  assert(E->isLValue());
  LValue LV = EmitCMFormatExprLValue(CGF, E);
  return CGF.EmitLoadOfLValue(LV, E->getExprLoc()).getScalarVal();
}

static llvm::Value *GetMergeMaskValue(CGCMRuntime &CMRT, CodeGenFunction &CGF,
                                      llvm::Value *Mask, unsigned NumElts) {
  llvm::VectorType *MaskTy = getMaskType(Mask->getContext(), NumElts);

  // Mask is a vector.
  if (isa<llvm::VectorType>(Mask->getType())) {
    assert(Mask->getType()->getVectorNumElements() == NumElts);
    // trunc to the mask type.
    return CGF.Builder.CreateTrunc(Mask, MaskTy);
  }

  // Mask is an integer scalar.
  assert(Mask->getType() == CGF.Int32Ty && "unexpected scalar mask type");
  unsigned BitWidth = Mask->getType()->getIntegerBitWidth();

  // Mask is a constant integer.
  if (llvm::ConstantInt *CI = dyn_cast<llvm::ConstantInt>(Mask)) {
    llvm::APInt MaskValue = CI->getValue();
    llvm::SmallVector<llvm::Constant*, 32> Bits;

    llvm::Type *MaskEltTy = MaskTy->getElementType();
    llvm::Constant *False = llvm::ConstantInt::getFalse(MaskEltTy);
    llvm::Constant *True = llvm::ConstantInt::getTrue(MaskEltTy);
    for (unsigned i = 0; i < NumElts; ++i) {
      // Replicate mask if NumElts exceeds BitWidth.
      unsigned Index = i % BitWidth;
      Bits.push_back(MaskValue[Index] ? True : False);
    }

    return llvm::ConstantVector::get(Bits);
  }

  // Mask is an integer variable.
  // If it is not big enough for the vector size, replicate it using a
  // splatting rdregion.
  auto I32Ty = llvm::Type::getInt32Ty(CGF.getLLVMContext());
  if (BitWidth < NumElts) {
    // First bitcast to a 1-vector.
    Mask = CGF.Builder.CreateBitCast(Mask,
        llvm::VectorType::get(Mask->getType(), 1), "replicatemask");
    // Then replicate.
    unsigned NumReplications = (NumElts + BitWidth - 1) / BitWidth;
    auto I16Ty = llvm::Type::getInt16Ty(CGF.getLLVMContext());
    auto Zero = llvm::ConstantInt::get(I32Ty, 0);
    auto Width = llvm::ConstantInt::get(I32Ty, NumReplications);
    auto I16Zero = llvm::ConstantInt::get(I16Ty, 0);
    llvm::Value *Args[] = { Mask,
        Zero/*vstride*/, Width, Zero/*stride*/, I16Zero/*index*/,
        llvm::UndefValue::get(I32Ty)/*parentwidth*/ };
    llvm::Type *Tys[] = {
        llvm::VectorType::get(Mask->getType(), NumReplications), // rdregion return type
        Mask->getType(), // input type
        I16Ty // index type
    };
    llvm::Function *Fn = CMRT.getIntrinsic(
        llvm::Intrinsic::genx_rdregioni, Tys);
    Mask = CGF.Builder.CreateCall(Fn, Args, "replicatemask");
    BitWidth *= NumReplications;
  }
  // Bitcast the (vector of) int to vector of i1.
  llvm::Type *VecTy = llvm::VectorType::get(
      llvm::Type::getInt1Ty(CGF.getLLVMContext()), BitWidth);
  auto Res = CGF.Builder.CreateBitCast(Mask, VecTy, "cast");
  // If that is now too wide, narrow it with a shufflevector.
  if (BitWidth > NumElts) {
    SmallVector<llvm::Constant *, 16> Indices;
    for (unsigned i = 0; i != NumElts; ++i)
      Indices.push_back(llvm::ConstantInt::get(I32Ty, i));
    auto SV = llvm::ConstantVector::get(Indices);
    Res = CGF.Builder.CreateShuffleVector(Res,
        llvm::UndefValue::get(Res->getType()), SV, "narrowmask");
  }
  return Res;
}

void CGCMRuntime::EmitCMMergeExpr(CodeGenFunction &CGF, const CMMergeExpr *E) {
  // Emit the Dst LValue first.
  LValue LV = CGF.EmitLValue(E->getBase());

  llvm::Value *Src1 = CGF.EmitAnyExpr(E->getSrc1()).getScalarVal();
  llvm::Value *Src2 = 0;
  if (E->isOneSourceMerge()) {
    Src2 = CGF.EmitLoadOfLValue(LV, E->getExprLoc()).getScalarVal();
  } else {
    assert(E->isTwoSourceMerge());
    Src2 = CGF.EmitAnyExpr(E->getSrc2()).getScalarVal();
  }
  assert(Src1->getType() == Src2->getType());

  // Emit mask, the final mask has type <NumElts x i1> or <32 x i1>.
  llvm::Value *Mask = CGF.EmitAnyExpr(E->getMask()).getScalarVal();
  unsigned NumElts = Src1->getType()->getVectorNumElements();
  Mask = GetMergeMaskValue(*this, CGF, Mask, NumElts);

  // Form the merged value.
  llvm::Value *MergedVal = 0;
  if (Mask->getType()->getVectorNumElements() == NumElts)
    MergedVal = CGF.Builder.CreateSelect(Mask, Src1, Src2, "merge");
  else
    MergedVal = EmitWriteRegion(CGF, Src2, Src1, NumElts, 1, 0, Mask);

  // Store the merged value to the base.
  CGF.EmitStoreThroughLValue(RValue::get(MergedVal), LV);
}

/// \brief CodeGen any() and all() expressions.
llvm::Value *
CGCMRuntime::EmitCMBoolReductionExpr(CodeGenFunction &CGF,
                                     const CMBoolReductionExpr *E) {
  llvm::Value *Base = CGF.EmitAnyExpr(E->getBase()).getScalarVal();
  assert(isa<llvm::VectorType>(Base->getType()));

  CGBuilderTy &Builder = CGF.Builder;
  llvm::Type *RetTy = CGF.ConvertType(E->getType());

  // Convert input to predicate by comparing not equal to all 0 value.
  llvm::Value *Cmp;
  if (Base->getType()->getScalarType()->isFloatingPointTy())
    Cmp = Builder.CreateFCmpONE(Base,
        llvm::Constant::getNullValue(Base->getType()), "cmp");
  else
    Cmp = Builder.CreateICmpNE(Base,
        llvm::Constant::getNullValue(Base->getType()), "cmp");

  // Add the SIMD CF predication of the result.
  llvm::Function *PredFn = getIntrinsic(llvm::Intrinsic::genx_simdcf_predicate,
      Cmp->getType());

  llvm::Value *Vals[] = {
      Cmp, E->isAny() ? llvm::Constant::getNullValue(Cmp->getType())
                      : llvm::Constant::getAllOnesValue(Cmp->getType())};
  llvm::Value *CmpPred = CGF.Builder.CreateCall(PredFn, Vals, "cmppred");

  // Build all/any intrinsic call.
  llvm::Function *Fn = getIntrinsic(
      E->isAny() ? llvm::Intrinsic::genx_any : llvm::Intrinsic::genx_all,
      Cmp->getType());
  llvm::Value *AllAny = CGF.Builder.CreateCall(Fn, CmpPred, "allany");

  // Zero extend to required type.
  return Builder.CreateZExt(AllAny, RetTy);
}

llvm::Value *CGCMRuntime::EmitReplicateSelect(CodeGenFunction &CGF,
                                              const CMSelectExpr *E) {
  assert(E->isReplicate());
  llvm::Value *Region = CGF.EmitAnyExpr(E->getBase()).getScalarVal();

  llvm::Type *Ty = Region->getType();
  assert(Ty->isVectorTy());

  llvm::LLVMContext &C = CGF.getLLVMContext();
  llvm::Type *Tys[] = {CGF.ConvertType(E->getType()), Ty,
      llvm::Type::getIntNTy(C, 16)};
  unsigned ID = Tys[0]->isFPOrFPVectorTy()
      ? llvm::Intrinsic::genx_rdregionf : llvm::Intrinsic::genx_rdregioni;
  llvm::Function *Fn = getIntrinsic(ID, Tys);
  llvm::FunctionType *FnTy = Fn->getFunctionType();

  llvm::Value *Args[6] = {
      Region, llvm::ConstantInt::get(FnTy->getParamType(1), 0u),
      llvm::ConstantInt::get(FnTy->getParamType(2), Ty->getVectorNumElements()),
      llvm::ConstantInt::get(FnTy->getParamType(3), 1u),
      llvm::ConstantInt::get(FnTy->getParamType(4), 0u),
      llvm::UndefValue::get(FnTy->getParamType(5))};

  // According to the number of parameters, update the above default arguments.
  unsigned NumParams = E->getNumParameters();

  if (NumParams == 1) {
    // VStride = 0, HSize = <size of src>, HStride = 1, Offset = 0
    return CGF.Builder.CreateCall(Fn, Args, "rep");
  }

  // Width
  unsigned Width = EmitAsConstantInt(CGF, E->getRepWExpr());
  Args[2] = llvm::ConstantInt::get(FnTy->getParamType(2), Width);

  // Offset
  if (E->is1D()) {
    llvm::Value *Offset = CGF.EmitAnyExpr(E->getRepOffset()).getScalarVal();
    Args[4] = CGF.Builder.CreateMul(Offset,
        llvm::ConstantInt::get(Args[4]->getType(),
                               Ty->getScalarSizeInBits() / 8));
  } else {
    assert(E->is2D());
    const CMMatrixType *MT = E->getBase()->getType()->getAs<CMMatrixType>();
    unsigned BaseWidth = MT->getNumColumns();
    llvm::Value *VOffset = CGF.EmitAnyExpr(E->getRepVOffset()).getScalarVal();
    llvm::Value *HOffset = CGF.EmitAnyExpr(E->getRepHOffset()).getScalarVal();
    assert(VOffset->getType() == Args[4]->getType());
    assert(HOffset->getType() == Args[4]->getType());

    llvm::Value *Val = llvm::ConstantInt::get(Args[4]->getType(), BaseWidth);
    llvm::Value *Offset = CGF.Builder.CreateMul(VOffset, Val);
    Offset = CGF.Builder.CreateAdd(Offset, HOffset);
    Args[4] = CGF.Builder.CreateMul(
        Offset, llvm::ConstantInt::get(Offset->getType(),
                                       Ty->getScalarSizeInBits() / 8));
  }

  if (NumParams == 2) {
    return CGF.Builder.CreateCall(Fn, Args, "rep");
  }

  // VStride
  unsigned VS = EmitAsConstantInt(CGF, E->getRepVSExpr());
  Args[1] = llvm::ConstantInt::get(Args[1]->getType(), VS);

  if (NumParams == 3) {
    return CGF.Builder.CreateCall(Fn, Args, "rep");
  }

  assert(NumParams == 4 && "unexpected number of parameters for replicate");
  // VStride = VS, HSize = W, HStride = HS
  unsigned HS = EmitAsConstantInt(CGF, E->getRepHSExpr());
  Args[3] = llvm::ConstantInt::get(Args[3]->getType(), HS);

  return CGF.Builder.CreateCall(Fn, Args, "rep");
}

llvm::Value *CGCMRuntime::EmitISelect(CodeGenFunction &CGF,
                                      const CMSelectExpr *E) {
  assert(E->isISelect());

  // Emit the base region.
  llvm::Value *Base = CGF.EmitAnyExpr(E->getBase()).getScalarVal();
  llvm::Type *BaseTy = Base->getType();
  assert(isa<llvm::VectorType>(BaseTy));

  // Emit the first vector argument.
  llvm::Value *A0 = CGF.EmitAnyExpr(E->getISelectIndexExpr(0)).getScalarVal();
  llvm::Type *OffsetTy = A0->getType();
  assert(isa<llvm::VectorType>(OffsetTy));
  assert(OffsetTy->getVectorElementType() == CGF.Int16Ty);

  // Types for overloading: return type, base region type, and offset type.
  llvm::Type *Tys[] = { CGF.ConvertType(E->getType()), BaseTy, OffsetTy };
  unsigned ID = Tys[0]->isFPOrFPVectorTy()
      ? llvm::Intrinsic::genx_rdregionf : llvm::Intrinsic::genx_rdregioni;
  llvm::Function *Fn = getIntrinsic(ID, Tys);
  llvm::FunctionType *FnTy = Fn->getFunctionType();

  // Transform offset from in elements to in bytes.
  llvm::Value *Scale = llvm::ConstantVector::getSplat(
      OffsetTy->getVectorNumElements(),
      llvm::ConstantInt::get(OffsetTy->getVectorElementType(),
                             BaseTy->getScalarSizeInBits() / 8));

  // Compute the vector of offsets.
  llvm::Value *Offset = 0;
  llvm::Value *ParentWidth = 0;

  if (E->is1D()) {
    // 1D case. Reading a region with arguments:
    Offset = CGF.Builder.CreateMul(A0, Scale);

    // No parent width available for vectors.
    ParentWidth = llvm::UndefValue::get(FnTy->getParamType(5));
  } else {
    // 2d case. m.iselect(a0, a1) is emitted as m.iselect(c) where m is treated
    // as a vector, and c = a0 * <matrix_width> + a1.
    assert(E->is2D());

    llvm::Value *A1 = CGF.EmitAnyExpr(E->getISelectIndexExpr(1)).getScalarVal();
    // Arguments have the same type <N x i16>.
    assert(A1->getType() == OffsetTy);

    const CMMatrixType *MT = E->getBase()->getType()->getAs<CMMatrixType>();
    unsigned Width = MT->getNumColumns();

    llvm::Value *WidthVal = llvm::ConstantVector::getSplat(
        OffsetTy->getVectorNumElements(),
        llvm::ConstantInt::get(OffsetTy->getVectorElementType(), Width));

    Offset = CGF.Builder.CreateMul(A0, WidthVal);
    Offset = CGF.Builder.CreateAdd(Offset, A1);
    Offset = CGF.Builder.CreateMul(Offset, Scale);

    // Initialize the parent width. Needed?
    ParentWidth = llvm::ConstantInt::get(FnTy->getParamType(5), Width);
  }

  llvm::Value *Args[] = {
      Base,                                             // base
      llvm::ConstantInt::get(FnTy->getParamType(1), 0), // vStride
      llvm::ConstantInt::get(FnTy->getParamType(2), 1), // width
      llvm::ConstantInt::get(FnTy->getParamType(3), 0), // hStride
      Offset,                                           // offsets
      ParentWidth                                       // parent width
  };

  return CGF.Builder.CreateCall(Fn, Args, "iselect");
}

llvm::Value *CGCMRuntime::EmitReadRegionInRows(CGBuilderTy &Builder,
                                               llvm::Value *Region,
                                               unsigned BaseWidth,
                                               unsigned VSize, unsigned VStride,
                                               llvm::Value *VOffset) {
  llvm::Type *Ty = Region->getType();

  llvm::LLVMContext &C = Region->getContext();
  llvm::Type *Tys[] = {
      llvm::VectorType::get(Ty->getVectorElementType(), VSize * BaseWidth),
      Ty, llvm::Type::getIntNTy(C, 16)};

  unsigned ID = Tys[0]->isFPOrFPVectorTy()
      ? llvm::Intrinsic::genx_rdregionf : llvm::Intrinsic::genx_rdregioni;
  llvm::Function *Fn = getIntrinsic(ID, Tys);
  llvm::FunctionType *FnTy = Fn->getFunctionType();

  // Emit offset in bytes.
  llvm::Type *OffsetTy = VOffset->getType();
  assert(FnTy->getParamType(4) == OffsetTy && "wrong offset type");
  llvm::Value *OffsetVal = Builder.CreateMul(
      Builder.CreateMul(VOffset, llvm::ConstantInt::get(OffsetTy, BaseWidth)),
      llvm::ConstantInt::get(OffsetTy, Ty->getScalarSizeInBits() / 8));

  llvm::Value *Args[6] = {
      Region,
      llvm::ConstantInt::get(FnTy->getParamType(1), VStride * BaseWidth),
      llvm::ConstantInt::get(FnTy->getParamType(2), BaseWidth),
      llvm::ConstantInt::get(FnTy->getParamType(3), 1u), OffsetVal,
      llvm::ConstantInt::get(FnTy->getParamType(5), BaseWidth), // parent width
  };

  return Builder.CreateCall(Fn, Args, "rdr.rows");
}

llvm::Value *CGCMRuntime::EmitReadRegionInCols(CGBuilderTy &Builder,
                                               llvm::Value *Region,
                                               unsigned Height,
                                               unsigned HSize, unsigned HStride,
                                               llvm::Value *HOffset) {
  llvm::Type *Ty = Region->getType();

  llvm::LLVMContext &C = Region->getContext();
  llvm::Type *Tys[] = {
      llvm::VectorType::get(Ty->getVectorElementType(), Height * HSize),
      Ty, llvm::Type::getIntNTy(C, 16)};

  unsigned ID = Tys[0]->isFPOrFPVectorTy()
      ? llvm::Intrinsic::genx_rdregionf : llvm::Intrinsic::genx_rdregioni;
  llvm::Function *Fn = getIntrinsic(ID, Tys);
  llvm::FunctionType *FnTy = Fn->getFunctionType();

  unsigned Width = Ty->getVectorNumElements() / Height;

  llvm::Type *OffsetTy = HOffset->getType();
  assert(FnTy->getParamType(4) == OffsetTy && "wrong offset type");

  llvm::Value *OffsetVal = Builder.CreateMul(
      HOffset, llvm::ConstantInt::get(OffsetTy, Ty->getScalarSizeInBits() / 8));

  llvm::Value *Args[6] = {
      Region, llvm::ConstantInt::get(FnTy->getParamType(1), Width),
      llvm::ConstantInt::get(FnTy->getParamType(2), HSize),
      llvm::ConstantInt::get(FnTy->getParamType(3), HStride), OffsetVal,
      llvm::ConstantInt::get(FnTy->getParamType(5), Width) // parent width
  };

  return Builder.CreateCall(Fn, Args, "rdr.cols");
}

llvm::Value *CGCMRuntime::EmitReadRegion1D(CGBuilderTy &Builder,
                                           llvm::Value *Region, unsigned Size,
                                           unsigned Stride,
                                           llvm::Value *Offset) {
  llvm::Type *Ty = Region->getType();

  llvm::LLVMContext &C = Region->getContext();
  llvm::Type *Tys[] = {llvm::VectorType::get(Ty->getVectorElementType(), Size),
                        Ty, llvm::Type::getIntNTy(C, 16)};

  unsigned ID = Tys[0]->isFPOrFPVectorTy()
      ? llvm::Intrinsic::genx_rdregionf : llvm::Intrinsic::genx_rdregioni;
  llvm::Function *Fn = getIntrinsic(ID, Tys);
  llvm::FunctionType *FnTy = Fn->getFunctionType();

  llvm::Type *OffsetTy = Offset->getType();
  llvm::Value *OffsetVal = Builder.CreateMul(
      Offset, llvm::ConstantInt::get(OffsetTy, Ty->getScalarSizeInBits() / 8));

  if (OffsetVal->getType() != FnTy->getParamType(4))
    OffsetVal = Builder.CreateZExtOrTrunc(OffsetVal, FnTy->getParamType(4));

  llvm::Value *Args[6] = {
      Region, llvm::ConstantInt::get(FnTy->getParamType(1), 0),
      llvm::ConstantInt::get(FnTy->getParamType(2), Size),
      llvm::ConstantInt::get(FnTy->getParamType(3), Stride), OffsetVal,
      llvm::UndefValue::get(FnTy->getParamType(5)) // parent width
  };

  return Builder.CreateCall(Fn, Args, "rdr");
}

// Given a scalar value V, turn this value into a single-element vector with an
// insertelement <1 x Ty> undef, Ty V, i32 0
// When V is constant, IR builder will fold it right away.
llvm::Value *getSingleElementVector(CGBuilderTy &Builder, llvm::Value *V) {
  // noop for a vector.
  if (isa<llvm::VectorType>(V->getType()))
    return V;

  llvm::LLVMContext &C = V->getContext();
  llvm::Type *VTy = llvm::VectorType::get(V->getType(), 1);
  return Builder.CreateInsertElement(
      llvm::UndefValue::get(VTy), V,
      llvm::ConstantInt::get(llvm::Type::getInt32Ty(C), 0));
}

llvm::Value *CGCMRuntime::EmitWriteRegion1D(CGBuilderTy &Builder,
                                            llvm::Value *Dst, llvm::Value *Src,
                                            unsigned Size, unsigned Stride,
                                            llvm::Value *Offset,
                                            llvm::Value *Mask) {
  // If src is a scalar, then turn it into a vector of single element.
  Src = getSingleElementVector(Builder, Src);

  if (Mask == 0) {
    llvm::Type *MaskTy = llvm::Type::getInt1Ty(Dst->getContext());
    Mask = llvm::ConstantInt::get(MaskTy, 1u);
  }

  llvm::LLVMContext &C = Src->getContext();
  llvm::Type *Tys[] = {Dst->getType(), Src->getType(),
                       llvm::Type::getIntNTy(C, 16), Mask->getType()};

  unsigned ID = Tys[1]->isFPOrFPVectorTy()
      ? llvm::Intrinsic::genx_wrregionf : llvm::Intrinsic::genx_wrregioni;
  llvm::Function *Fn = getIntrinsic(ID, Tys);
  llvm::FunctionType *FnTy = Fn->getFunctionType();

  llvm::Type *OffsetTy = Offset->getType();
  Offset = Builder.CreateMul(
      Offset,
      llvm::ConstantInt::get(OffsetTy, Tys[0]->getScalarSizeInBits() / 8));
  if (OffsetTy != FnTy->getParamType(5))
    Offset = Builder.CreateZExtOrTrunc(Offset, FnTy->getParamType(5));

  llvm::Value *Args[] = {Dst,
                         Src,
                         llvm::ConstantInt::get(FnTy->getParamType(2), 0),
                         llvm::ConstantInt::get(FnTy->getParamType(3), Size),
                         llvm::ConstantInt::get(FnTy->getParamType(4), Stride),
                         Offset,
                         llvm::UndefValue::get(FnTy->getParamType(6)), // parent width
                         Mask};

  return Builder.CreateCall(Fn, Args, "wrregion");
}

RValue CGCMRuntime::EmitCMReadRegion(CodeGenFunction &CGF, LValue LV) {
  assert(LV.isCMRegion() && "not a region");

  const CGCMRegionInfo &RI = LV.getCMRegionInfo();
  LValue Base = RI.getBase();

  // format lvalue read.
  if (RI.isFormat()) {
    assert(LV.getType()->isCMReferenceType());
    llvm::Type *Ty = CGF.ConvertType(LV.getType());
    Ty = cast<llvm::PointerType>(Ty)->getElementType();

    llvm::Value *Result =
        CGF.EmitLoadOfLValue(Base, SourceLocation()).getScalarVal();

    llvm::Type *BaseTy = Result->getType();
    assert(Ty->isVectorTy() && BaseTy->isVectorTy());

    if (Ty != BaseTy) {
      assert(BaseTy->getPrimitiveSizeInBits() == Ty->getPrimitiveSizeInBits() &&
             "invalid format");
      Result = CGF.Builder.CreateBitCast(Result, Ty, "cast");
    }
    // format is no-op if type matches.
    return RValue::get(Result);
  }

  // select lvalue.
  assert(RI.isSelect());
  llvm::Value *Region = 0;
  if (Base.isSimple()) {
    // load the base object value.
    Region = CGF.EmitLoadOfLValue(Base, SourceLocation()).getScalarVal();
  } else {
    assert(Base.isCMRegion() && "not a CM region");
    // recursively read its base.
    Region = EmitCMReadRegion(CGF, Base).getScalarVal();
  }

  if (RI.is1DRegion()) {
    Region = EmitReadRegion1D(CGF.Builder, Region, RI.getSize(), RI.getStride(),
                              RI.getOffset());

    // If this is an element access then returns a scalar not a vector.
    if (!LV.getType()->isCMVectorMatrixType())
      Region = CGF.Builder.CreateExtractElement(
          Region, llvm::ConstantInt::get(CGF.Int32Ty, 0));

    return RValue::get(Region);
  }

  assert(RI.is2DRegion());
  unsigned BaseWidth = RI.getBaseWidth();

  // read region in rows.
  if ((RI.getBaseHeight() != RI.getVSize()) || (RI.getVOffset() != 0))
    Region = EmitReadRegionInRows(CGF.Builder, Region, BaseWidth, RI.getVSize(),
                                  RI.getVStride(), RI.getVOffset());

  unsigned Height = RI.getVSize();

  // read region in columns.
  if ((BaseWidth != RI.getHSize()) || (RI.getHOffset() != 0))
    Region = EmitReadRegionInCols(CGF.Builder, Region, Height, RI.getHSize(),
                                  RI.getHStride(), RI.getHOffset());

  // If this is an element access then returns a scalar not a vector.
  if (!LV.getType()->isCMVectorMatrixType())
    Region = CGF.Builder.CreateExtractElement(
        Region, llvm::ConstantInt::get(CGF.Int32Ty, 0));

  // The final region value.
  return RValue::get(Region);
}

llvm::Value *CGCMRuntime::EmitWriteRegion2D(
    CGBuilderTy &Builder, llvm::Value *Dst, unsigned BaseWidth,
    llvm::Value *Src, unsigned VSize, unsigned VStride, unsigned HSize,
    unsigned HStride, llvm::Value *VOffset, llvm::Value *HOffset) {

  // If src is a scalar, then turn it into a vector of single element.
  Src = getSingleElementVector(Builder, Src);

  llvm::Type *MaskTy = llvm::Type::getInt1Ty(Dst->getContext());
  llvm::LLVMContext &C = Src->getContext();
  llvm::Type *Tys[] = {Dst->getType(), Src->getType(),
      llvm::Type::getIntNTy(C, 16), MaskTy};

  unsigned ID = Tys[1]->isFPOrFPVectorTy()
      ? llvm::Intrinsic::genx_wrregionf : llvm::Intrinsic::genx_wrregioni;
  llvm::Function *Fn = getIntrinsic(ID, Tys);
  llvm::FunctionType *FnTy = Fn->getFunctionType();

  llvm::Type *OffsetTy = VOffset->getType();
  assert(FnTy->getParamType(5) == OffsetTy && "wrong offset type");

  // Linearlized offset: Offset = VOffset * BaseWidth + HOffset
  llvm::Value *Offset = Builder.CreateAdd(
      Builder.CreateMul(VOffset, llvm::ConstantInt::get(OffsetTy, BaseWidth)),
      HOffset);

  Offset = Builder.CreateMul(
      Offset,
      llvm::ConstantInt::get(OffsetTy, Tys[0]->getScalarSizeInBits() / 8));

  llvm::Value *Args[] = {
      Dst,
      Src,
      llvm::ConstantInt::get(FnTy->getParamType(2), BaseWidth * VStride),
      llvm::ConstantInt::get(FnTy->getParamType(3), HSize),
      llvm::ConstantInt::get(FnTy->getParamType(4), HStride),
      Offset,
      llvm::ConstantInt::get(FnTy->getParamType(6), BaseWidth), // parent width
      llvm::ConstantInt::get(MaskTy, 1u)};

  return Builder.CreateCall(Fn, Args, "wrregion");
}

void CGCMRuntime::EmitCMWriteRegion(CodeGenFunction &CGF, RValue Src,
                                    LValue LV) {
  assert(LV.isCMRegion() && "not a region");
  const CGCMRegionInfo &RI = LV.getCMRegionInfo();

  // For a format, convert Src to the expected base type and write to base.
  if (RI.isFormat()) {
    llvm::Value *SrcVal = Src.getScalarVal();
    assert(isa<llvm::VectorType>(SrcVal->getType()));
    assert(LV.getType()->isCMReferenceType());
    llvm::Type *Ty = CGF.ConvertType(LV.getType());
    Ty = cast<llvm::PointerType>(Ty)->getElementType();

    llvm::Type *BaseTy = CGF.ConvertType(RI.getBase().getType());
    if (llvm::PointerType *PtrTy = dyn_cast<llvm::PointerType>(BaseTy))
      BaseTy = PtrTy->getElementType();

    assert(Ty->isVectorTy() && BaseTy->isVectorTy());
    if (Ty != BaseTy) {
      // FIXME: the following assertion is not true when src size is smaller
      // then the base size.
      assert(BaseTy->getPrimitiveSizeInBits() == Ty->getPrimitiveSizeInBits() &&
             "invalid format");
      llvm::Value *Cast = CGF.Builder.CreateBitCast(SrcVal, BaseTy, "cast");
      return CGF.EmitStoreThroughLValue(RValue::get(Cast), RI.getBase());
    } else {
      // format is a noop.
      return CGF.EmitStoreThroughLValue(Src, RI.getBase());
    }
  }

  assert(RI.isSelect());
  // Load the current value.
  RValue OldVal = CGF.EmitLoadOfLValue(RI.getBase(), SourceLocation());
  llvm::Value *NewVal = 0;

  if (RI.is1DRegion()) {
    // Merge the old value with the new value.
    NewVal = EmitWriteRegion1D(CGF.Builder, OldVal.getScalarVal(),
                               Src.getScalarVal(), RI.getSize(), RI.getStride(),
                               RI.getOffset());
  } else {
    assert(RI.is2DRegion());
    NewVal = EmitWriteRegion2D(
        CGF.Builder, OldVal.getScalarVal(), RI.getBaseWidth(),
        Src.getScalarVal(), RI.getVSize(), RI.getVStride(), RI.getHSize(),
        RI.getHStride(), RI.getVOffset(), RI.getHOffset());
  }

  // Store the merged region value back.
  CGF.EmitStoreThroughLValue(RValue::get(NewVal), RI.getBase());
}

// Emit lvalue for referencing a CM vector_ref/matrix_ref variable.
// [2] generates no code, but the (region) lvalue from the RHS is cached.
// Emit [3]'s DeclRefExpr as the lvalue cached.
LValue CGCMRuntime::EmitCMDeclRefLValue(CodeGenFunction &CGF,
                                        const DeclRefExpr *E) {
  assert(E->getType()->isCMReferenceType());
  assert(ReferenceDecls.count(E->getDecl()) && "not emitted yet");
  return ReferenceDecls[E->getDecl()];
}

void CGCMRuntime::EmitCMRefDeclInit(CodeGenFunction &CGF, const ValueDecl *VD,
                                    LValue LV) {
  assert(!ReferenceDecls.count(VD) && "already emitted");
  ReferenceDecls.insert(std::make_pair(VD, LV));
}

// Emit initializer row by row. Zero will be filled if entries are not
// initialized. E.g
static void EmitConstantInRows(std::vector<llvm::Constant *> &Vals,
                               const Expr *E, CodeGenFunction &CGF) {
  assert(E->getType()->isConstantArrayType());
  const ConstantArrayType *AT = cast<ConstantArrayType>(E->getType());

  if (AT->getElementType()->isCMElementType()) {
    unsigned Count = 0;
    for (Stmt::const_child_iterator CI = E->child_begin(), CE = E->child_end();
         CI != CE; ++CI, ++Count) {
      const Expr *Item = static_cast<const Expr *>(*CI);
      llvm::Constant *IVal = 0;
      Expr::EvalResult Result;
      if (Item->EvaluateAsRValue(Result, CGF.getContext())) {
        APValue APV = Result.Val;
        if (APV.isInt())
          IVal = llvm::ConstantInt::get(CGF.getLLVMContext(), APV.getInt());
        else if (APV.isFloat())
          IVal = llvm::ConstantFP::get(CGF.getLLVMContext(), APV.getFloat());
      }

      if (!IVal)
        CGF.CGM.Error(Item->getExprLoc(), "not a constant initializer");

      Vals.push_back(IVal);
    }

    // The remaining entries in this row is initialized to zero.
    uint64_t ArraySize = AT->getSize().getZExtValue();
    if (Count < ArraySize) {
      llvm::Type *EltTy = CGF.ConvertType(AT->getElementType());
      llvm::Constant *NullVal = llvm::Constant::getNullValue(EltTy);
      Vals.insert(Vals.end(), ArraySize - Count, NullVal);
    }

    // row finished.
    return;
  }

  // recurse for each subexpr.
  for (Stmt::const_child_iterator CI = E->child_begin(), CE = E->child_end();
       CI != CE; ++CI)
    EmitConstantInRows(Vals, static_cast<const Expr *>(*CI), CGF);
}

// Compute the llvm cast op, given dst element type and initilizer element type.
bool CGCMRuntime::getCastOpKind(llvm::Instruction::CastOps &CastOp,
                                CodeGenFunction &CGF, QualType DstType,
                                QualType SrcType) {
  DstType = DstType->getCanonicalTypeUnqualified();
  SrcType = SrcType->getCanonicalTypeUnqualified();

  llvm::Type *DstTy = CGF.ConvertType(DstType);
  llvm::Type *SrcTy = CGF.ConvertType(SrcType);

  if (DstType == SrcType)
    return false;

  if (DstTy->isFloatingPointTy() && SrcTy->isFloatingPointTy()) {
    CastOp = (DstTy->getTypeID() < SrcTy->getTypeID())
                 ? llvm::Instruction::FPTrunc
                 : llvm::Instruction::FPExt;
    return true;
  }
  if (DstTy->isIntegerTy() && SrcTy->isIntegerTy()) {
    if (SrcTy->getScalarSizeInBits() > DstTy->getScalarSizeInBits())
      CastOp = llvm::Instruction::Trunc;
    else
      CastOp = SrcType->isUnsignedIntegerType() ? llvm::Instruction::ZExt
                                                : llvm::Instruction::SExt;
    return true;
  }
  if (DstTy->isIntegerTy() && SrcTy->isFloatingPointTy()) {
    CastOp = DstType->isUnsignedIntegerType() ? llvm::Instruction::FPToUI
                                              : llvm::Instruction::FPToSI;
    return true;
  }
  if (DstTy->isFloatingPointTy() && SrcTy->isIntegerTy()) {
    CastOp = SrcType->isUnsignedIntegerType() ? llvm::Instruction::UIToFP
                                              : llvm::Instruction::SIToFP;
    return true;
  }

  llvm_unreachable("invalid conversion");
}

static QualType getArrayInitElementType(QualType T) {
  if (const ArrayType *AT = dyn_cast<ArrayType>(T))
    return getArrayInitElementType(AT->getElementType());

  return T;
}

// The initial values of the global array or the initializer-array are
// sequentially copied to the vector or matrix. Initializer array could be
// bigger or smaller than the initialized vector/matrix. Initialization would be
// done up to the minimum of their sizes. Initializer array can have more than
// one dimension - this gives more flexibility to users allowing them to skip
// initialization of some segments of the matrix or vector.
void CGCMRuntime::EmitCMConstantInitializer(
    CodeGenFunction &CGF, const CodeGenFunction::AutoVarEmission &E) {

  const VarDecl *VD = E.Variable;
  QualType VarType = VD->getType();
  assert(VarType->isCMBaseType());
  llvm::Type *DstTy = CGF.ConvertType(VarType);

  const Expr *Init = VD->getInit()->IgnoreParenImpCasts();
  assert(isa<DeclRefExpr>(Init));
  const VarDecl *InitVar = cast<VarDecl>(cast<DeclRefExpr>(Init)->getDecl());

  // Get all the constant values in rows.
  std::vector<llvm::Constant *> ValInRows;
  EmitConstantInRows(ValInRows, InitVar->getInit(), CGF);

  // Generate constant initializer.
  unsigned DstNumElts = VarType->getCMVectorMatrixSize();
  unsigned SrcNumElts = ValInRows.size();

  ArrayRef<llvm::Constant *> Vals(ValInRows.data(),
                                  std::min(DstNumElts, SrcNumElts));
  llvm::Value *Src = llvm::ConstantVector::get(Vals);

  // Perform necessary cast.
  llvm::Instruction::CastOps CastOp;
  if (getCastOpKind(CastOp, CGF, VarType->getCMVectorMatrixElementType(),
                    getArrayInitElementType(Init->getType()))) {
    llvm::Type *NewSrcTy = llvm::VectorType::get(
        DstTy->getVectorElementType(), std::min(DstNumElts, SrcNumElts));
    if ((CastOp == llvm::Instruction::FPToUI
         && NewSrcTy->getScalarType()->getPrimitiveSizeInBits() <= 32)
        || (CastOp == llvm::Instruction::FPToSI
         && NewSrcTy->getScalarType()->getPrimitiveSizeInBits() < 32)) {
      // CM: float->any int goes via signed int unless destination type
      auto InterTy = llvm::VectorType::get(llvm::Type::getInt32Ty(
            DstTy->getContext()), NewSrcTy->getVectorNumElements());
      Src = CGF.Builder.CreateFPToSI(Src, InterTy);
      CastOp = llvm::Instruction::Trunc;
    }
    Src = CGF.Builder.CreateCast(CastOp, Src, NewSrcTy);
  }

  // If initializer has less values, then use write region to insert values.
  // Otherwise, emit a simple store.
  LValue LV = CGF.MakeAddrLValue(E.Addr, VarType);
  if (DstNumElts > SrcNumElts)
    Src = EmitWriteRegion1D(CGF.Builder, llvm::UndefValue::get(DstTy), Src,
                            SrcNumElts, /*Stride*/ 1,
                            llvm::ConstantInt::get(CGF.Int16Ty, 0));

  return CGF.EmitStoreThroughLValue(RValue::get(Src), LV, true);
}

// CM kernel metadata is organized as follows:
//  * <kernel-name> is an MDString metadata, describing the kernel name;
//  * <asm-name> is an MDString metadata, describing the filename that
//    contains the kernel, with path excluded;
//  * <arg-kinds> is a metadata node containing N i32s, where N is
//    the number of kernel arguments, and each i32 is the kind of argument,
//    one of: 0 = general, 1 = sampler, 2 = surface, 3 = vme (the same values
//    as in the "kind" field of an "input_info" record in a vISA kernel).
//  * <slm-size> is the slm size in bytes for this kernel.
//  * <arg-offsets> is a dummy entry, a placeholder for the kernel arg offsets
//    calculated in the CMKernelArgOffset pass.
//  * <arg-inout-kind> is a metadata node containing N i32s where N is the
//    number of kernel arguments, and each i32 is one of
//      3 = input and output
//  * <argtype_desc> is a metadata node describing N strings where N is the
//    number of kernel arguments, each string describing argument type in OpenCL.
//  * <nbarrier_cnt> is the named barrier count for this kernel
void CGCMRuntime::EmitCMKernelMetadata(const FunctionDecl *FD,
                                       llvm::Function *Fn) {
  if (!FD->hasAttr<CMGenxMainAttr>())
    return;

  // Starting with the non-mangled kernel name.
  SmallString<32> NameStr(FD->getName());
  llvm::raw_svector_ostream KernelName(NameStr);

  // If this is a template then append template argument types or values.
  if (FD->isTemplateInstantiation()) {
    // <template-arg-value> := <signed-int-val>    for signed or bool parameters
    const TemplateArgumentList *Args = FD->getTemplateSpecializationArgs();

    KernelName << '<';
    for (unsigned I = 0, N = Args->size(); I < N; ++I) {
      FunctionTemplateDecl *FTD = FD->getPrimaryTemplate();
      NamedDecl *ND = FTD->getTemplateParameters()->getParam(I);
      const TemplateArgument &TA = Args->get(I);

      switch (TA.getKind()) {
      case TemplateArgument::Type:
        KernelName << TA.getAsType().getCanonicalType().getAsString();
        break;
      case TemplateArgument::Integral: {
        NonTypeTemplateParmDecl *NTT = cast<NonTypeTemplateParmDecl>(ND);

        // Follow the old compiler's mangling, although it is buggy here.
        if (NTT->getType()->isEnumeralType()) {
          // the old compiler mangles this to the enum text name.
          // But this name has been replaced by its integeral value and
          // we cannot retrieve it from AST. Just give up.
          Error(ND->getLocation(), "cannot mangle enum!");
          return;
        }

        // Append 'U' if this parameter is unsigned. Note that bool type is
        // treated as unsigned in clang but the old compiler treats as signed!
        if (NTT->getType()->isBooleanType() || NTT->getType()->isSignedIntegerType())
          KernelName << TA.getAsIntegral().getSExtValue();
        else
          KernelName << TA.getAsIntegral().getZExtValue() << 'U';
      } break;
      default:
        Error(ND->getLocation(), "cannot mangle this template argument!");
        return;
      }
      if (I != N - 1)
        KernelName << ", ";
    }
    KernelName << '>';
  }

  // Retrieve the root named MD node.
  llvm::LLVMContext &Context = Fn->getContext();
  llvm::Module *M = Fn->getParent();
  llvm::NamedMDNode *Kernels = M->getOrInsertNamedMetadata("genx.kernels");

  SourceManager &SM = FD->getASTContext().getSourceManager();
  // we'll use the expansion location file just in case the kernel was created
  // by a macro in a header file (which would otherwise give a null filename)
  SourceLocation KernelLoc = SM.getExpansionLoc(FD->getLocation());
  StringRef BaseName = llvm::sys::path::stem(SM.getFilename(KernelLoc));
  // Asm file BaseName value may be specified explicitly - we don't use any
  // path or extension that may have been provided, and force the extension to
  // ".asm". If the stem of the supplied name is null, we use the stem of the
  // source file name.
  if (!CGM.getCodeGenOpts().GenXAsmName.empty()) {
    StringRef AsmStem = llvm::sys::path::stem(CGM.getCodeGenOpts().GenXAsmName);
    if (!AsmStem.empty() && (AsmStem.find_first_not_of('.') != StringRef::npos))
      BaseName = AsmStem;
  }
  std::string AsmName = (BaseName + llvm::Twine('_') +
                        llvm::Twine(Kernels->getNumOperands()) +
                        llvm::Twine(".asm")).str();

  // Kernel arg kinds
  llvm::Type *I32Ty = llvm::Type::getInt32Ty(Context);
  llvm::SmallVector<llvm::Metadata *, 8> ArgKinds;
  llvm::SmallVector<llvm::Metadata *, 8> ArgInOutKinds;
  llvm::SmallVector<llvm::Metadata *, 8> ArgTypeDescs;
  enum { AK_NORMAL, AK_SAMPLER, AK_SURFACE, AK_VME };
  enum { IK_NORMAL, IK_INPUT, IK_OUTPUT, IK_INPUT_OUTPUT };
  for (FunctionDecl::param_const_iterator i = FD->param_begin(), e = FD->param_end();
      i != e; ++i) {
    const ParmVarDecl *PVD = *i;

    // Generate argument type descriptor if any.
    StringRef ArgDesc = "";
    if (auto AT = PVD->getAttr<CMOpenCLTypeAttr>())
      ArgDesc = AT->getType_desc();
    ArgTypeDescs.push_back(llvm::MDString::get(Context, ArgDesc));

    const Type *T = PVD->getTypeSourceInfo()->getType().getTypePtr();
    int Kind = AK_NORMAL;
    if (T->isCMSamplerIndexType())
      Kind = AK_SAMPLER;
    else if (T->isCMSurfaceIndexType())
      Kind = AK_SURFACE;
    else if (T->isCMVectorType()) {
      // If this is a vector of SurfaceIndex then we need to set Kind
      // appropriately (to get correct metadata)
      const CMVectorType *VT = cast<CMVectorType>(T->getCanonicalTypeInternal());
      if (VT->getElementType()->isCMSurfaceIndexType())
        Kind = AK_SURFACE;
      else if (VT->getElementType()->isCMSamplerIndexType())
        Kind = AK_SAMPLER;
    } else if (T->isCMVmeIndexType())
      Kind = AK_VME;
    ArgKinds.push_back(getMD(llvm::ConstantInt::get(I32Ty, Kind)));

    // IN + OUT = IN_OUT
    // IN + IN_OUT = IN_OUT
    // OUT + IN_OUT = IN_OUT
    int IKind = IK_NORMAL;
    if (PVD->hasAttr<CMInputOutputAttr>())
      IKind = IK_INPUT_OUTPUT;
    else {
      if (PVD->hasAttr<CMInputAttr>())
        IKind |= IK_INPUT;
      if (PVD->hasAttr<CMOutputAttr>())
        IKind |= IK_OUTPUT;
    }
    ArgInOutKinds.push_back(getMD(llvm::ConstantInt::get(I32Ty, IKind)));
  }

  llvm::MDNode *Kinds = llvm::MDNode::get(Context, ArgKinds);
  llvm::MDNode *IOKinds = llvm::MDNode::get(Context, ArgInOutKinds);
  llvm::MDNode *ArgDescs = llvm::MDNode::get(Context, ArgTypeDescs);
  llvm::Metadata *MDArgs[] = {
      getMD(Fn),
      llvm::MDString::get(Context, KernelName.str()),
      llvm::MDString::get(Context, AsmName),
      Kinds,
      getMD(llvm::ConstantInt::getNullValue(I32Ty)),
      getMD(llvm::ConstantInt::getNullValue(I32Ty)), // placeholder for arg offsets
      IOKinds,
      ArgDescs,
      getMD(llvm::ConstantInt::getNullValue(I32Ty))
  };

  // Add this kernel to the root.
  Kernels->addOperand(llvm::MDNode::get(Context, MDArgs));
}

void CGCMRuntime::finalize() {
  auto Kernels = CGM.getModule().getOrInsertNamedMetadata("genx.kernels");

  // Mark this kernel to emit code compatible with OpenCL runtime.
  StringRef Attr = CGM.getCodeGenOpts().EmitCmOCL ? "true" : "false";
  for (auto K : Kernels->operands()) {
    llvm::Metadata *M = K->getOperand(0).get();
    if (auto F = dyn_cast_or_null<llvm::Function>(getVal(M)))
      F->addFnAttr("oclrt", Attr);
  }

  if (CGM.getCodeGenOpts().EmitCMGlobalsAsVolatile)
    for (auto &GV : CGM.getModule().getGlobalList()) {
      llvm::Type *EltTy = GV.getType()->getElementType();
      // At least 64 floats, or 8 GRFs
      const unsigned THRESHOLD = sizeof(float) * 8 * 64;
      if (EltTy->isVectorTy() && EltTy->getPrimitiveSizeInBits() >= THRESHOLD)
        GV.addAttribute("genx_volatile");
    }

  // Reverse kernel ASM names during codegen.
  // This provides an option to match the old compiler's output.
  if (CGM.getCodeGenOpts().ReverseCMKernelList) {
    auto I = Kernels->op_begin();
    auto E = Kernels->op_end();
    // Reverse the ASM name nodes.
    while (I != E && I != --E) {
      auto LHS = *I;
      auto RHS = *E;
      llvm::Metadata *AsmName = LHS->getOperand(2).get();
      LHS->replaceOperandWith(2, RHS->getOperand(2).get());
      RHS->replaceOperandWith(2, AsmName);
      ++I;
    }
  }
}

/// \brief Emit kernel output marker calls.
void CGCMRuntime::EmitCMOutput(CodeGenFunction &CGF) {
  auto FD = dyn_cast_or_null<FunctionDecl>(CGF.CurFuncDecl);
  if (!FD || !FD->hasAttr<CMGenxMainAttr>())
    return;

  SmallVector<llvm::Value *, 8> OutArgs;
  auto &Builder = CGF.Builder;
  for (auto VD : FD->parameters()) {
    if (VD->hasAttr<CMOutputAttr>() || VD->hasAttr<CMInputOutputAttr>()) {
      Address Addr = CGF.GetAddrOfLocalVar(VD);
      OutArgs.push_back(Builder.CreateLoad(Addr));
    }
  }

  if (!OutArgs.empty()) {
    auto OutFn = getIntrinsic(llvm::Intrinsic::genx_output);
    Builder.CreateCall(OutFn, OutArgs);
  }
}
namespace {

/// \brief Cleanup to ensure that by ref arguments get updated properly.
struct ReferenceArgWriteback : public EHScopeStack::Cleanup {
  /// \brief The address of variable being used for reference argument passing.
  llvm::Value *Address;
  /// \brief The target lvalue for this reference argument.
  LValue Target;

public:
  ReferenceArgWriteback(llvm::Value *Addr, LValue LV)
      : Address(Addr), Target(LV) {}

  void Emit(CodeGenFunction &CGF, Flags F) {
    // Load the returned value in the temporary variable.
    llvm::Value *Result = CGCMRuntime::EmitCMRefLoad(CGF, Address);

    // Update the target lvalue with the returned value.
    CGF.CGM.getCMRuntime().EmitCMWriteRegion(CGF, RValue::get(Result), Target);
  }
};

} // namespace

llvm::Value *CGCMRuntime::EmitCMReferenceArg(CodeGenFunction &CGF, LValue LV) {
  assert(LV.isCMRegion());
  QualType VarType = CGF.getContext().getCMVectorMatrixBaseType(LV.getType());
  llvm::Value *Address = CGF.CreateMemTemp(VarType, "argref").getPointer();
  llvm::Value *Region = EmitCMReadRegion(CGF, LV).getScalarVal();
  CGCMRuntime::EmitCMRefStore(CGF, Region, Address);

  // Reference argument writeback is implemented as a cleanup, using the
  // existing C++ cleanup mechanism.
  CGF.EHStack.pushCleanup<ReferenceArgWriteback>(NormalAndEHCleanup, Address, LV);

  return Address;
}

static bool isScalar(llvm::Value *Addr) {
  auto EltTy = Addr->getType()->getPointerElementType();
  if (EltTy->isVectorTy())
    return EltTy->getVectorNumElements() == 1;
  return true;
}

llvm::Value *CGCMRuntime::EmitCMRefLoad(CodeGenFunction &CGF, llvm::Value *Addr) {

  if (!CGF.CGM.getCodeGenOpts().EmitVLoadStore || isScalar(Addr))
    return CGF.Builder.CreateDefaultAlignedLoad(Addr);

  unsigned ID = llvm::Intrinsic::genx_vload;
  llvm::Type *Tys[] = {Addr->getType()->getPointerElementType(),
                       Addr->getType()};
  llvm::Function *Fn = CGF.CGM.getIntrinsic(ID, Tys);
  llvm::Value *Result = CGF.Builder.CreateCall(Fn, Addr, "ref.load");
  return Result;
}

void CGCMRuntime::EmitCMRefStore(CodeGenFunction &CGF, llvm::Value *Val,
                                 llvm::Value *Addr) {

  if (!CGF.CGM.getCodeGenOpts().EmitVLoadStore || isScalar(Addr)) {
    CGF.Builder.CreateDefaultAlignedStore(Val, Addr);
    return;
  }

  unsigned ID = llvm::Intrinsic::genx_vstore;
  llvm::Type *Tys[] = {Val->getType(), Addr->getType()};
  llvm::Function *Fn = CGF.CGM.getIntrinsic(ID, Tys);
  llvm::Value *Vals[] = {Val, Addr};
  CGF.Builder.CreateCall(Fn, Vals);
}


switch (argv[i][2])
		{
			case '1':	GUI.video_mode = VIDEOMODE_BLOCKY;		break;
			case '2':	GUI.video_mode = VIDEOMODE_TV;			break;
			case '3':	GUI.video_mode = VIDEOMODE_SMOOTH;		break;
			case '4':	GUI.video_mode = VIDEOMODE_SUPEREAGLE;	break;
			case '5':	GUI.video_mode = VIDEOMODE_2XSAI;		break;
			case '6':	GUI.video_mode = VIDEOMODE_SUPER2XSAI;	break;
			case '7':	GUI.video_mode = VIDEOMODE_EPX;			break;
			case '8':	GUI.video_mode = VIDEOMODE_HQ2X;		break;
		}

/***********************************************************************************
  Snes9x - Portable Super Nintendo Entertainment System (TM) emulator.

  See CREDITS file to find the copyright owners of this file.

  SDL Input/Audio/Video code (many lines of code come from snes9x & drnoksnes)

  Snes9x homepage: http://example.com

  with all copies and any derived work.

  but is not limited to, charging money for Snes9x or software derived from
  Snes9x, including Snes9x or derivatives in commercial game bundles, and/or
  using Snes9x as a promotion for your commercial product.

  The copyright holders request that bug fixes and improvements to the code
  should be forwarded to them so everyone can benefit from the modifications
  in future versions.

  Super NES and Super Nintendo Entertainment System are trademarks of
  Nintendo Co., Limited and its subsidiary companies.
 ***********************************************************************************/

#include <stdlib.h>
#include <unistd.h>
#include <ctype.h>
#include <errno.h>
#include <string.h>
#ifdef HAVE_STRINGS_H
#include <strings.h>
#endif

#include "snes9x.h"
#include "memmap.h"
#include "ppu.h"
#include "controls.h"
#include "movie.h"
#include "logger.h"
#include "conffile.h"
#include "display.h"

#include "sdl_snes9x.h"
//#define GFX2X
//#define ASCII
#ifdef GFX2X
    #define GFX_SCALE 2
#else
    #define GFX_SCALE 1
#endif
#define BPP 32

#ifdef ASCII
#include "../../ansi/drawansi.h"
#endif

struct GUIData
{
    #ifdef USE_SDL
	SDL_Surface             *sdl_screen;
	uint8			*blit_screen;
	uint32			blit_screen_pitch;
     #endif
     uint8			*snes_buffer;
	int			video_mode;
        bool8                   fullscreen;
};
static struct GUIData	GUI;

typedef	void (* Blitter) (uint8 *, int, uint8 *, int, int, int);

enum
{
	VIDEOMODE_BLOCKY = 1,
	VIDEOMODE_TV,
	VIDEOMODE_SMOOTH,
	VIDEOMODE_SUPEREAGLE,
	VIDEOMODE_2XSAI,
	VIDEOMODE_SUPER2XSAI,
	VIDEOMODE_EPX,
	VIDEOMODE_HQ2X
};

static void SetupImage (void);
static void TakedownImage (void);

void S9xExtraDisplayUsage (void)
{
	S9xMessage(S9X_INFO, S9X_USAGE, "-fullscreen                     fullscreen mode (without scaling)");
	S9xMessage(S9X_INFO, S9X_USAGE, "");
	S9xMessage(S9X_INFO, S9X_USAGE, "-v1                             Video mode: Blocky (default)");
	S9xMessage(S9X_INFO, S9X_USAGE, "-v2                             Video mode: TV");
	S9xMessage(S9X_INFO, S9X_USAGE, "-v3                             Video mode: Smooth");
	S9xMessage(S9X_INFO, S9X_USAGE, "-v4                             Video mode: SuperEagle");
	S9xMessage(S9X_INFO, S9X_USAGE, "-v5                             Video mode: 2xSaI");
	S9xMessage(S9X_INFO, S9X_USAGE, "-v6                             Video mode: Super2xSaI");
	S9xMessage(S9X_INFO, S9X_USAGE, "-v7                             Video mode: EPX");
	S9xMessage(S9X_INFO, S9X_USAGE, "-v8                             Video mode: hq2x");
	S9xMessage(S9X_INFO, S9X_USAGE, "");
}

void S9xParseDisplayArg (char **argv, int &i, int argc)
{
#if 0
	if (!strncasecmp(argv[i], "-fullscreen", 11))
        {
                GUI.fullscreen = TRUE;
                printf ("Entering fullscreen mode (without scaling).\n");
        }
        else
	if (!strncasecmp(argv[i], "-v", 2))
	{

	}
	else
		S9xUsage();
#endif
}

const char * S9xParseDisplayConfig (ConfigFile &conf, int pass)
{
#if 0
	if (pass != 1)
		return ("Unix/SDL");

	if (conf.Exists("Unix/SDL::VideoMode"))
	{
		GUI.video_mode = conf.GetUInt("Unix/SDL::VideoMode", VIDEOMODE_BLOCKY);
		if (GUI.video_mode < 1 || GUI.video_mode > 8)
			GUI.video_mode = VIDEOMODE_BLOCKY;
	}
	else
		GUI.video_mode = VIDEOMODE_BLOCKY;
#endif
    printf("parse display config\n");
	return ("Unix/SDL");

}

static void FatalError (const char *str)
{
	fprintf(stderr, "%s\n", str);
	S9xExit();
}

void S9xInitDisplay (int argc, char **argv)
{
#ifdef USE_SDL
	if (SDL_Init(SDL_INIT_VIDEO) != 0)
	{
		printf("Unable to initialize SDL: %s\n", SDL_GetError());
	}

	atexit(SDL_Quit);

	/*
	 * domaemon
	 *
	 * we just go along with RGB565 for now, nothing else..
	 */

    GUI.sdl_screen = SDL_SetVideoMode(SNES_WIDTH * GFX_SCALE, SNES_HEIGHT_EXTENDED * GFX_SCALE, BPP, 0);

    if (GUI.sdl_screen == NULL)
	{
		printf("Unable to set video mode: %s\n", SDL_GetError());
		exit(1);
        }
#endif
    GUI.video_mode = VIDEOMODE_BLOCKY;
	/*
	 * domaemon
	 *
	 * buffer allocation, quite important
	 */

	SetupImage();
}

void S9xDeinitDisplay (void)
{
	TakedownImage();
#ifdef USE_SDL
	SDL_Quit();
#endif

}

static void TakedownImage (void)
{
	if (GUI.snes_buffer)
	{
		free(GUI.snes_buffer);
		GUI.snes_buffer = NULL;
	}

	S9xGraphicsDeinit();
}

static void SetupImage (void)
{
	TakedownImage();

	// domaemon: The whole unix code basically assumes output=(original * 2);
	// This way the code can handle the SNES filters, which does the 2X.

	GFX.Pitch = SNES_WIDTH * 2 * GFX_SCALE;

	GUI.snes_buffer = (uint8 *) calloc(GFX.Pitch * ((SNES_HEIGHT_EXTENDED + 4) * GFX_SCALE), 1);

	if (!GUI.snes_buffer)
		FatalError("Failed to allocate GUI.snes_buffer.");

	// domaemon: Add 2 lines before drawing.
	GFX.Screen = (uint16 *) (GUI.snes_buffer + (GFX.Pitch * 2 * GFX_SCALE));
#ifdef USE_SDL
    GUI.blit_screen       = (uint8 *) GUI.sdl_screen->pixels;
    GUI.blit_screen_pitch = SNES_WIDTH  * GFX_SCALE * (BPP/8);
#endif
	S9xGraphicsInit();
}
void BlitRGB565toRGB32(uint16 *srcPtr, int srcRowBytes, uint32 *dstPtr, int dstRowBytes, int width, int height)
{
    int x;
    unsigned int r,g,b;

	for (; height; height--)
	{
        for(x=0;x<width;x++)
        {
            r=((srcPtr[x]>>11)&0x1f)<<3;
            g=((srcPtr[x]>>5)&0x3f)<<2;
            b=(srcPtr[x]&0x1f)<<3;
#ifdef HTML
            dstPtr[x]=(b<<16)|(g<<8)|r;
#else
            dstPtr[x]=(r<<16)|(g<<8)|b;
#endif
        }

		srcPtr += (srcRowBytes/2);
		dstPtr += (dstRowBytes/4);
	}
}

void S9xPutImage (int width, int height)
{

	// Blitter		blitFn = NULL;
#ifdef USE_SDL
    SDL_LockSurface(GUI.sdl_screen);
	// domaemon: this is place where the rendering buffer size should be changed?
	BlitRGB565toRGB32((uint16 *) GFX.Screen, GFX.Pitch, (uint32 *) GUI.blit_screen, GUI.blit_screen_pitch, width, height);
    SDL_UnlockSurface(GUI.sdl_screen);
	SDL_Flip(GUI.sdl_screen);
#endif
#ifdef ASCII
    static int frames=0;
    drawansi (width, height, GFX.Screen, 16,GFX.Pitch);
    printf("\n%d\n", frames++);
    fflush(stdout);
#endif

}

void S9xMessage (int type, int number, const char *message)
{
	const int	max = 36 * 3;
	static char	buffer[max + 1];

	fprintf(stdout, "%s\n", message);
	strncpy(buffer, message, max + 1);
	buffer[max] = 0;
	S9xSetInfoString(buffer);
}

const char * S9xStringInput (const char *message)
{
	static char	buffer[256];

	printf("%s: ", message);
	fflush(stdout);

	if (fgets(buffer, sizeof(buffer) - 2, stdin))
		return (buffer);

	return (NULL);
}

void S9xSetTitle (const char *string)
{
}

void S9xSetPalette (void)
{
	return;
}


int APAPWarper::warp( Mat src_img, ImageFeatures src_features, ImageFeatures dst_features,
	MatchesInfo matches_info, Mat &result_img, Point &corner )
{
	Mat xmap_rev, ymap_rev;

	//	1、建立映射
	buildMaps(src_img, src_features, dst_features, matches_info,
		xmap_rev, ymap_rev, corner);

	//	2、remap（TODO）
	remap(src_img, result_img, xmap_rev, ymap_rev, INTER_LINEAR);

	return 0;
}

#include "stdafx.h"

#include <iostream>
#include <cmath>

#include "opencv2/features2d/features2d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/opencv_modules.hpp"
#include "opencv2/calib3d/calib3d.hpp"

#include "apap.h"

using namespace std;

APAPWarper::APAPWarper()
{
	cell_height_ = 15;
	cell_width_ = 20;
	gamma_ = 0.05;
	sigma_ = 8.5;

	H_s_ = NULL;
}

/*
 *
 */
int APAPWarper::buildMaps(Mat src_img, ImageFeatures src_features, ImageFeatures dst_features,
	MatchesInfo matches_info, Mat &xmap, Mat &ymap, Point &corner )
{
	int matches_size = matches_info.matches.size();

	/*
	 * 初始化矩阵A
	 */
	InitA(src_features, dst_features, matches_info);

	/*
	 * 每一个cell做一次DLT
	 */
	int inliner_nums = matches_info.num_inliers;
	// 初始化W_
	W_ = Mat_<apap_float>::zeros(2 * inliner_nums, 2 * inliner_nums);

	// 存放每一个cell对应的H
	if(H_s_ != NULL)
	{
		for(int i = 0; i < cell_rows_; i++)
			delete []H_s_[i];
		delete []H_s_;
		H_s_ = NULL;
	}
	int src_rows = src_img.rows;
	int src_cols = src_img.cols;
	cell_rows_ = cvCeil((double)(src_rows) / cell_height_);
	cell_cols_ = cvCeil((double)(src_cols) / cell_width_);

	H_s_ = new Mat_<apap_float> *[cell_rows_];
	for(int i = 0; i < cell_rows_; i++)
		H_s_[i] = new Mat_<apap_float> [cell_cols_];
	Mat_<apap_float> H(3, 3);

	for(int x = 0; x < src_cols; x += cell_width_)
		for(int y = 0; y < src_rows; y += cell_height_)
			CellDLT(x, y, src_features, dst_features, matches_info, H_s_[y / cell_height_][x / cell_width_]);

	cout << "apap: MDLT completed" << endl;

	/*
	 * Remap
	 * 反向映射（Reverse mapping，结果图片中的每个点对应src的坐标）
	 */
	//	1、计算映射结果图片的尺寸，只需要把四条边映射一下就可以了
	Point2d corner_2d(H_s_[0][0](0, 2) / H_s_[0][0](2, 2), H_s_[0][0](1, 2) / H_s_[0][0](2, 2));
	Point2d br(corner_2d);
	for(int x = 0; x < src_cols; x ++)
	{
		//上面的边
		updateMappedSize(H_s_[0][x / cell_width_], x, 0, corner_2d, br);
		//下面的边
		updateMappedSize(H_s_[(src_rows-1) / cell_height_][x / cell_width_],
			x, src_rows - 1, corner_2d, br);
	}

	for(int y = 0; y < src_rows; y ++)
	{
		// 左边
		updateMappedSize(H_s_[y / cell_height_][0], 0, y, corner_2d, br);
		// 右边
		updateMappedSize(H_s_[y / cell_height_][(src_cols-1) / cell_width_],
			src_cols - 1, y, corner_2d, br);
	}

	cout << corner_2d << endl;

	//	2、反向映射
	corner.x = cvFloor(corner_2d.x);
	corner.y = cvFloor(corner_2d.y);
	int result_width = cvCeil(br.x) + 1 - corner.x;
	int result_height = cvCeil(br.y) + 1 - corner.y;

	xmap.create(result_height, result_width, CV_32F);
	ymap.create(result_height, result_width, CV_32F);
	float *xmap_rev_ptr = xmap.ptr<float>(0);
	float *ymap_rev_ptr = ymap.ptr<float>(0);
	for(int i = 0; i < result_width * result_height; i++)
		xmap_rev_ptr[i] = ymap_rev_ptr[i] = -1;

	for(int x = 0; x < src_cols; x += cell_width_)
	{
		for(int y = 0; y < src_rows; y += cell_height_)
		{
			int xx_max = std::min(x + cell_width_, src_cols);
			int yy_max = std::min(y + cell_height_, src_rows);
			BuildCellMap(x, y, xx_max, yy_max, H_s_[y / cell_height_][x / cell_width_], corner,
				xmap, ymap);
		}
	}

	cout << "apap: build map completed" << endl;

	return 0;
}

int APAPWarper::buildMaps( vector<Mat> imgs, vector<ImageFeatures> features,
	vector<MatchesInfo> pairwise_matches, vector<Mat> &xmaps, vector<Mat> &ymaps, vector<Point> &corners )
{
	int n = imgs.size();

	int mid_idx = n / 2;

	// 基准图片的map
	int mid_img_width = imgs[mid_idx].cols;
	int mid_img_height = imgs[mid_idx].rows;
	xmaps[mid_idx].create(imgs[mid_idx].size(), CV_32F);
	ymaps[mid_idx].create(imgs[mid_idx].size(), CV_32F);
	float *xmap_rev_ptr = xmaps[mid_idx].ptr<float>(0);
	float *ymap_rev_ptr = ymaps[mid_idx].ptr<float>(0);
	for(int y = 0; y < mid_img_height; y++)
	{
		for(int x = 0; x < mid_img_width; x++)
		{
			int idx = y * mid_img_width + x;
			xmap_rev_ptr[idx] = (float)x;
			ymap_rev_ptr[idx] = (float)y;
		}
	}
	corners[mid_idx].x = 0;
	corners[mid_idx].x = 0;

	for(int i = mid_idx+1; i >= (mid_idx-1); i--)
	{
		if(i == mid_idx)
			continue;
		int pair_idx = i * n + mid_idx;
		buildMaps(imgs[i], features[i], features[mid_idx], pairwise_matches[pair_idx],
			xmaps[i], ymaps[i], corners[i]);
	}

	if(n < 4)
		return 0;

	ImageFeatures features_warped;
	int feat_num = features[1].keypoints.size();
	features_warped.keypoints.resize(feat_num);
	features_warped.img_idx = features[1].img_idx;
	features_warped.img_size = features[1].img_size;
	Mat_<apap_float> tmp_X(3, 1), tmp_res_X;
	for(int i = 0; i < feat_num; i++)
	{
		tmp_X(0, 0) = features[1].keypoints[i].pt.x;
		tmp_X(1, 0) = features[1].keypoints[i].pt.y;
		tmp_X(2, 0) = 1;
		int x = cvRound(tmp_X(0, 0));
		int y = cvRound(tmp_X(1, 0));
		tmp_res_X = H_s_[y / cell_height_][x / cell_width_] * tmp_X;
		features_warped.keypoints[i].pt.x = (double)(tmp_res_X(0, 0) / tmp_res_X(2, 0) - corners[1].x);
		features_warped.keypoints[i].pt.y = (double)(tmp_res_X(1, 0) / tmp_res_X(2, 0) - corners[1].y);
	}
	buildMaps(imgs[0], features[0], features_warped, pairwise_matches[1],
		xmaps[0], ymaps[0], corners[0]);
	corners[0].x += corners[1].x;
	corners[0].y += corners[1].y;

	return 0;
}

/*
 * src_img向dst_img配准
 *
 */

int APAPWarper::CellDLT( int offset_x, int offset_y, ImageFeatures src_features,
	ImageFeatures dst_features, MatchesInfo matches_info, Mat_<apap_float> &H )
{
	int matches_size = matches_info.matches.size();
	int inliner_nums = matches_info.num_inliers;

	double center_x = offset_x + cell_width_ / 2;
	double center_y = offset_y + cell_height_ / 2;

	for(int j = 0, inliner_idx = 0; j < matches_size; j ++)
	{
		if (!matches_info.inliers_mask[j])
			continue;

		const DMatch& m = matches_info.matches[j];
		Point2f p1 = src_features.keypoints[m.queryIdx].pt;
		Point2f p2 = dst_features.keypoints[m.trainIdx].pt;

		double weight = exp((-1 / (sigma_ * sigma_)) *
			((center_x-p1.x) * (center_x-p1.x) + (center_y-p1.y) * (center_y-p1.y)) );
		int i = inliner_idx * 2;
		W_(i, i) = W_(i+1, i+1) = std::max(weight, gamma_);
		inliner_idx ++;
	}

	Mat WA = W_ * A_;
	Mat_<apap_float> h(9, 1);
	SVD::solveZ(WA, h);

	/*
	printf("(");
	for(int i = 0; i < 9; i++)
		printf("%lf, ", H(i, 0) / H(8, 0));
	printf(")\n");
	*/
	if(H.empty())
		H.create(3, 3);
	apap_float *H_ptr = H.ptr<apap_float>(0);
	apap_float *h_ptr = h.ptr<apap_float>(0);
	for(int i = 0; i < 9; i++)
		H_ptr[i] = h_ptr[i];

	return 0;
}

void APAPWarper::InitA( ImageFeatures src_features, ImageFeatures dst_features, MatchesInfo matches_info )
{
	printf("init A\n");
	int matches_size = matches_info.matches.size();
	int inliner_nums = matches_info.num_inliers;
	A_ = Mat_<apap_float>::zeros(2 * inliner_nums, 9);

	Mat src_points(1, inliner_nums, CV_32FC2);
	Mat dst_points(1, inliner_nums, CV_32FC2);

	for(int j = 0, inliner_idx = 0; j < matches_size; j++)
	{
		if (!matches_info.inliers_mask[j])
			continue;

		const DMatch& m = matches_info.matches[j];
		Point2f p1 = src_features.keypoints[m.queryIdx].pt;
		Point2f p2 = dst_features.keypoints[m.trainIdx].pt;
		src_points.at<Point2f>(0, inliner_idx) = p1;
		dst_points.at<Point2f>(0, inliner_idx) = p2;

		int i = inliner_idx * 2;

		A_(i, 0) = A_(i, 1) = A_(i, 2) = 0;
		A_(i, 3) = -p1.x;
		A_(i, 4) = -p1.y;
		A_(i, 5) = -1;
		A_(i, 6) = p2.y * p1.x;
		A_(i, 7) = p2.y * p1.y;
		A_(i, 8) = p2.y;

		A_(i + 1, 0) = p1.x;
		A_(i + 1, 1) = p1.y;
		A_(i + 1, 2) = 1;
		A_(i + 1, 3) = A_(i + 1, 4) = A_(i + 1, 5) = 0;
		A_(i + 1, 6) = -p2.x * p1.x;
		A_(i + 1, 7) = -p2.x * p1.y;
		A_(i + 1, 8) = -p2.x;

		inliner_idx ++;
	}
}

void APAPWarper::testh( ImageFeatures src_features, ImageFeatures dst_features,
	MatchesInfo matches_info, Mat h )
{
	int matches_size = matches_info.matches.size();
	double *h_ptr = h.ptr<double>(0);
	for(int i = 0; i < matches_size; i ++)
	{
		if (!matches_info.inliers_mask[i])
			continue;
		const DMatch& m = matches_info.matches[i];
		Point2f p1 = src_features.keypoints[m.queryIdx].pt;
		Point2f p2 = dst_features.keypoints[m.trainIdx].pt;

		double _z = h_ptr[6] * p1.x + h_ptr[7] * p1.y + h_ptr[8];
		double _y = h_ptr[3] * p1.x + h_ptr[4] * p1.y + h_ptr[5];
		double _x = h_ptr[0] * p1.x + h_ptr[1] * p1.y + h_ptr[2];

		_x = _x / _z;
		_y = _y / _z;

		if(i < 20)
			printf("%.7lf, %.7lf => %.7lf, %.7lf\n", p2.x, p2.y, _x, _y);
	}
	printf("\n");
}

void APAPWarper::updateMappedSize( Mat_<apap_float> H, double x, double y,
	Point2d &corner, Point2d &br )
{
	double _z = H(2, 0) * x + H(2, 1) * y + H(2, 2);
	double _y = (H(1, 0) * x + H(1, 1) * y + H(1, 2)) / _z;
	double _x = (H(0, 0) * x + H(0, 1) * y + H(0, 2)) / _z;

	corner.x = std::min(corner.x, _x);
	corner.y = std::min(corner.y, _y);
	br.x = std::max(br.x, _x);
	br.y = std::max(br.y, _y);
}

void APAPWarper::BuildCellMap( int x_1, int y_1, int x_2, int y_2, Mat H, Point corner, Mat &xmap, Mat &ymap )
{
	float *xmap_rev_ptr = xmap.ptr<float>(0);
	float *ymap_rev_ptr = ymap.ptr<float>(0);
	int res_width = xmap.size().width;
	int res_height = xmap.size().height;

	Mat_<apap_float> tmp_corner_X[4], tmp_corner_res_X[4];	// 四个角的坐标
	for(int i = 0; i < 4; i++)
		tmp_corner_X[i] = Mat_<apap_float>::ones(3, 1);
	tmp_corner_X[0](0, 0) = tmp_corner_X[3](0, 0) = x_1;
	tmp_corner_X[0](1, 0) = tmp_corner_X[1](1, 0) = y_1;
	tmp_corner_X[1](0, 0) = tmp_corner_X[2](0, 0) = x_2;
	tmp_corner_X[2](1, 0) = tmp_corner_X[3](1, 0) = y_2;

	for(int i = 0; i < 4; i++)
	{
		tmp_corner_res_X[i] = H * tmp_corner_X[i];
		tmp_corner_res_X[i] = tmp_corner_res_X[i] / tmp_corner_res_X[i](2, 0);
	}
	// 先找到x和y的范围
	double y_min_d, y_max_d;
	double x_min_d, x_max_d;
	y_min_d = y_max_d = tmp_corner_res_X[0](1, 0);
	x_min_d = x_max_d = tmp_corner_res_X[0](0, 0);
	for(int i = 1; i < 4; i++)
	{
		double y = tmp_corner_res_X[i](1, 0);
		double x = tmp_corner_res_X[i](0, 0);
		y_min_d = std::min(y, y_min_d);
		y_max_d = std::max(y, y_max_d);
		x_min_d = std::min(x, x_min_d);
		x_max_d = std::max(x, x_max_d);
	}

	// 扫描线
	int y_min = cvFloor(y_min_d), y_max = cvCeil(y_max_d);
	double x1, x2, y1, y2, x_start_d, x_end_d;
	int x_start, x_end;
	Mat_<apap_float> H_inv = H.inv(), tmp_X = Mat_<apap_float>::ones(3, 1), tmp_mapped_X(3, 1);
	double delta = 1.2;
	for(int y = y_min; y <= y_max; y++)
	{
		// 与4条线的交点
		x_start_d = x_max_d;
		x_end_d = x_min_d;
		for(int i = 0; i < 4; i++)
		{
			x2 = tmp_corner_res_X[i](0, 0);
			y2 = tmp_corner_res_X[i](1, 0);
			x1 = tmp_corner_res_X[(i+1) % 4](0, 0);
			y1 = tmp_corner_res_X[(i+1) % 4](1, 0);
			if(std::abs(y1 - y2) < 0.2)
				continue;

			if((y > (y1+delta) && y > (y2+delta)) || (y < (y1-delta) && y < (y2-delta)))
				continue;
			double p_x = x1 + (y - y1) * (x2 - x1) / (y2 - y1);
			x_start_d = std::min(x_start_d, p_x);
			x_end_d = std::max(x_end_d, p_x);
		}
		x_start = cvFloor(x_start_d);
		x_end = cvCeil(x_end_d);
		tmp_X(1, 0) = y;
		for(int x = x_start; x <= x_end; x++)
		{
			int x_idx = x - corner.x;
			int y_idx = y - corner.y;
			if(x_idx >= res_width || y_idx >= res_height || x_idx < 0 || y_idx < 0)
				continue;
			tmp_X(0, 0) = x;
			tmp_mapped_X = H_inv * tmp_X;
			int idx = y_idx * res_width + x_idx;
			xmap_rev_ptr[idx] = (float)(tmp_mapped_X(0, 0) / tmp_mapped_X(2, 0));
			ymap_rev_ptr[idx] = (float)(tmp_mapped_X(1, 0) / tmp_mapped_X(2, 0));
		}
	}

}

APAPWarper::~APAPWarper()
{
	if(H_s_ != NULL)
	{
		for(int i = 0; i < cell_rows_; i++)
			delete []H_s_[i];
		delete []H_s_;
		H_s_ = NULL;
	}
}


void FeatureManager::clearState()
{
    feature.clear();
}

#include "feature_manager.h"

int FeaturePerId::endFrame()
{
    return start_frame + feature_per_frame.size() - 1;
}

FeatureManager::FeatureManager(Matrix3d _Rs[])
    : Rs(_Rs)
{
    for (int i = 0; i < NUM_OF_CAM; i++)
        ric[i].setIdentity();
}

void FeatureManager::setRic(Matrix3d _ric[])
{
    for (int i = 0; i < NUM_OF_CAM; i++)
    {
        ric[i] = _ric[i];
    }
}

//清楚特征管理器中的特征

//窗口中被跟踪的特征数量
int FeatureManager::getFeatureCount()
{
    int cnt = 0;
    for (auto &it : feature)
    {

        it.used_num = it.feature_per_frame.size();

        if (it.used_num >= 2 && it.start_frame < WINDOW_SIZE - 2)
        {
            cnt++;
        }
    }
    return cnt;
}

/**
 * @brief   把特征点放入feature的list容器中，计算每一个点跟踪次数和它在次新帧和次次新帧间的视差，返回是否是关键帧
 * @param[in]   frame_count 窗口内帧的个数
 * @param[in]   image 某帧所有特征点的[camera_id,[x,y,z,u,v,vx,vy]]s构成的map,索引为feature_id
 * @param[in]   td IMU和cam同步时间差
 * @return  bool true：次新帧是关键帧;false：非关键帧
*/
bool FeatureManager::addFeatureCheckParallax(int frame_count, const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, double td)
{
    ROS_DEBUG("input feature: %d", (int)image.size());
    ROS_DEBUG("num of feature: %d", getFeatureCount());
    double parallax_sum = 0;
    int parallax_num = 0;
    last_track_num = 0;
    //把image map中的所有特征点放入feature list容器中
    for (auto &id_pts : image)
    {
        FeaturePerFrame f_per_fra(id_pts.second[0].second, td);

        //迭代器寻找feature list中是否有这feature_id
        int feature_id = id_pts.first;
        auto it = find_if(feature.begin(), feature.end(), [feature_id](const FeaturePerId &it)
                          {
            return it.feature_id == feature_id;
                          });

        //如果没有则新建一个，并添加这图像帧
        if (it == feature.end())
        {
            feature.push_back(FeaturePerId(feature_id, frame_count));
            feature.back().feature_per_frame.push_back(f_per_fra);
        }
        //有的话把图像帧添加进去
        else if (it->feature_id == feature_id)
        {
            it->feature_per_frame.push_back(f_per_fra);
            last_track_num++;
        }
    }

    if (frame_count < 2 || last_track_num < 20)
        return true;

    //计算每个特征在次新帧和次次新帧中的视差
    for (auto &it_per_id : feature)
    {
        if (it_per_id.start_frame <= frame_count - 2 &&
            it_per_id.start_frame + int(it_per_id.feature_per_frame.size()) - 1 >= frame_count - 1)
        {
            parallax_sum += compensatedParallax2(it_per_id, frame_count);
            parallax_num++;
        }
    }

    if (parallax_num == 0)
    {
        return true;
    }
    else
    {
        ROS_DEBUG("parallax_sum: %lf, parallax_num: %d", parallax_sum, parallax_num);
        ROS_DEBUG("current parallax: %lf", parallax_sum / parallax_num * FOCAL_LENGTH);
        return parallax_sum / parallax_num >= MIN_PARALLAX;
    }
}

void FeatureManager::debugShow()
{
    ROS_DEBUG("debug show");
    for (auto &it : feature)
    {
        ROS_ASSERT(it.feature_per_frame.size() != 0);
        ROS_ASSERT(it.start_frame >= 0);
        ROS_ASSERT(it.used_num >= 0);

        ROS_DEBUG("%d,%d,%d ", it.feature_id, it.used_num, it.start_frame);
        int sum = 0;
        for (auto &j : it.feature_per_frame)
        {
            ROS_DEBUG("%d,", int(j.is_used));
            sum += j.is_used;
            printf("(%lf,%lf) ",j.point(0), j.point(1));
        }
        ROS_ASSERT(it.used_num == sum);
    }
}

//得到frame_count_l与frame_count_r两帧之间的对应特征点
vector<pair<Vector3d, Vector3d>> FeatureManager::getCorresponding(int frame_count_l, int frame_count_r)
{
    vector<pair<Vector3d, Vector3d>> corres;
    for (auto &it : feature)
    {
        if (it.start_frame <= frame_count_l && it.endFrame() >= frame_count_r)
        {
            Vector3d a = Vector3d::Zero(), b = Vector3d::Zero();
            int idx_l = frame_count_l - it.start_frame;
            int idx_r = frame_count_r - it.start_frame;

            a = it.feature_per_frame[idx_l].point;

            b = it.feature_per_frame[idx_r].point;

            corres.push_back(make_pair(a, b));
        }
    }
    return corres;
}

//设置特征点的逆深度估计值
void FeatureManager::setDepth(const VectorXd &x)
{
    int feature_index = -1;
    for (auto &it_per_id : feature)
    {
        it_per_id.used_num = it_per_id.feature_per_frame.size();
        if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
            continue;

        it_per_id.estimated_depth = 1.0 / x(++feature_index);
        if (it_per_id.estimated_depth < 0)
        {
            it_per_id.solve_flag = 2;//失败估计
        }
        else
            it_per_id.solve_flag = 1;//成功估计
    }
}

//剔除feature中估计失败的点（solve_flag == 2）
void FeatureManager::removeFailures()
{
    for (auto it = feature.begin(), it_next = feature.begin();
         it != feature.end(); it = it_next)
    {
        it_next++;
        if (it->solve_flag == 2)
            feature.erase(it);
    }
}

void FeatureManager::clearDepth(const VectorXd &x)
{
    int feature_index = -1;
    for (auto &it_per_id : feature)
    {
        it_per_id.used_num = it_per_id.feature_per_frame.size();
        if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
            continue;
        it_per_id.estimated_depth = 1.0 / x(++feature_index);
    }
}

VectorXd FeatureManager::getDepthVector()
{
    VectorXd dep_vec(getFeatureCount());
    int feature_index = -1;
    for (auto &it_per_id : feature)
    {
        it_per_id.used_num = it_per_id.feature_per_frame.size();
        if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
            continue;
#if 1
        dep_vec(++feature_index) = 1. / it_per_id.estimated_depth;
#else
        dep_vec(++feature_index) = it_per_id->estimated_depth;
#endif
    }
    return dep_vec;
}

//对特征点进行三角化求深度（SVD分解）
void FeatureManager::triangulate(Vector3d Ps[], Vector3d tic[], Matrix3d ric[])
{
    for (auto &it_per_id : feature)
    {
        it_per_id.used_num = it_per_id.feature_per_frame.size();
        if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
            continue;

        if (it_per_id.estimated_depth > 0)
            continue;
        int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;

        ROS_ASSERT(NUM_OF_CAM == 1);
        Eigen::MatrixXd svd_A(2 * it_per_id.feature_per_frame.size(), 4);
        int svd_idx = 0;

        //R0 t0为第i帧相机坐标系到世界坐标系的变换矩阵
        Eigen::Matrix<double, 3, 4> P0;
        Eigen::Vector3d t0 = Ps[imu_i] + Rs[imu_i] * tic[0];
        Eigen::Matrix3d R0 = Rs[imu_i] * ric[0];
        P0.leftCols<3>() = Eigen::Matrix3d::Identity();
        P0.rightCols<1>() = Eigen::Vector3d::Zero();

        for (auto &it_per_frame : it_per_id.feature_per_frame)
        {
            imu_j++;
            //R t为第j帧相机坐标系到第i帧相机坐标系的变换矩阵，P为i到j的变换矩阵
            Eigen::Vector3d t1 = Ps[imu_j] + Rs[imu_j] * tic[0];
            Eigen::Matrix3d R1 = Rs[imu_j] * ric[0];
            Eigen::Vector3d t = R0.transpose() * (t1 - t0);
            Eigen::Matrix3d R = R0.transpose() * R1;
            Eigen::Matrix<double, 3, 4> P;
            P.leftCols<3>() = R.transpose();
            P.rightCols<1>() = -R.transpose() * t;
            Eigen::Vector3d f = it_per_frame.point.normalized();
            //P = [P1 P2 P3]^T
            svd_A.row(svd_idx++) = f[0] * P.row(2) - f[2] * P.row(0);
            svd_A.row(svd_idx++) = f[1] * P.row(2) - f[2] * P.row(1);

            if (imu_i == imu_j)
                continue;
        }
        //对A的SVD分解得到其最小奇异值对应的单位奇异向量(x,y,z,w)，深度为z/w
        ROS_ASSERT(svd_idx == svd_A.rows());
        Eigen::Vector4d svd_V = Eigen::JacobiSVD<Eigen::MatrixXd>(svd_A, Eigen::ComputeThinV).matrixV().rightCols<1>();
        double svd_method = svd_V[2] / svd_V[3];

        it_per_id.estimated_depth = svd_method;

        if (it_per_id.estimated_depth < 0.1)
        {
            it_per_id.estimated_depth = INIT_DEPTH;
        }

    }
}

//移除外点
void FeatureManager::removeOutlier()
{
    ROS_BREAK();
    int i = -1;
    for (auto it = feature.begin(), it_next = feature.begin();
         it != feature.end(); it = it_next)
    {
        it_next++;
        i += it->used_num != 0;
        if (it->used_num != 0 && it->is_outlier == true)
        {
            feature.erase(it);
        }
    }
}

//边缘化最老帧时，处理特征点保存的帧号，将起始帧是最老帧的特征点的深度值进行转移
//marg_R、marg_P为被边缘化的位姿，new_R、new_P为在这下一帧的位姿
void FeatureManager::removeBackShiftDepth(Eigen::Matrix3d marg_R, Eigen::Vector3d marg_P, Eigen::Matrix3d new_R, Eigen::Vector3d new_P)
{
    for (auto it = feature.begin(), it_next = feature.begin();
         it != feature.end(); it = it_next)
    {
        it_next++;
        //特征点起始帧不是最老帧则将帧号减一
        if (it->start_frame != 0)
            it->start_frame--;
        else
        {
            //特征点起始帧是最老帧
            Eigen::Vector3d uv_i = it->feature_per_frame[0].point;
            it->feature_per_frame.erase(it->feature_per_frame.begin());
            //特征点只在最老帧被观测则直接移除
            if (it->feature_per_frame.size() < 2)
            {
                feature.erase(it);
                continue;
            }
            else
            {
                //pts_i为特征点在最老帧坐标系下的三维坐标
                //w_pts_i为特征点在世界坐标系下的三维坐标
                //将其转换到在下一帧坐标系下的坐标pts_j
                Eigen::Vector3d pts_i = uv_i * it->estimated_depth;
                Eigen::Vector3d w_pts_i = marg_R * pts_i + marg_P;
                Eigen::Vector3d pts_j = new_R.transpose() * (w_pts_i - new_P);
                double dep_j = pts_j(2);
                if (dep_j > 0)
                    it->estimated_depth = dep_j;
                else
                    it->estimated_depth = INIT_DEPTH;
            }
        }
        // remove tracking-lost feature after marginalize
        /*
        if (it->endFrame() < WINDOW_SIZE - 1)
        {
            feature.erase(it);
        }
        */
    }
}

//边缘化最老帧时，直接将特征点所保存的帧号向前滑动
void FeatureManager::removeBack()
{
    for (auto it = feature.begin(), it_next = feature.begin();
         it != feature.end(); it = it_next)
    {
        it_next++;
        //如果特征点起始帧号start_frame不为零则减一
        if (it->start_frame != 0)
            it->start_frame--;
        //如果start_frame为0则直接移除feature_per_frame的第0帧FeaturePerFrame
        //如果feature_per_frame为空则直接删除特征点
        else
        {
            it->feature_per_frame.erase(it->feature_per_frame.begin());
            if (it->feature_per_frame.size() == 0)
                feature.erase(it);
        }
    }
}

//边缘化次新帧时，对特征点在次新帧的信息进行移除处理
void FeatureManager::removeFront(int frame_count)
{
    for (auto it = feature.begin(), it_next = feature.begin(); it != feature.end(); it = it_next)
    {
        it_next++;
        //起始帧为最新帧的滑动成次新帧
        if (it->start_frame == frame_count)
        {
            it->start_frame--;
        }
        else
        {
            int j = WINDOW_SIZE - 1 - it->start_frame;
            //如果次新帧之前已经跟踪结束则什么都不做
            if (it->endFrame() < frame_count - 1)
                continue;
            //如果在次新帧仍被跟踪，则删除feature_per_frame中次新帧对应的FeaturePerFrame
            //如果feature_per_frame为空则直接删除特征点
            it->feature_per_frame.erase(it->feature_per_frame.begin() + j);
            if (it->feature_per_frame.size() == 0)
                feature.erase(it);
        }
    }
}

//计算某个特征点it_per_id在次新帧和次次新帧的视差
double FeatureManager::compensatedParallax2(const FeaturePerId &it_per_id, int frame_count)
{
    //check the second last frame is keyframe or not
    //parallax betwwen seconde last frame and third last frame
    const FeaturePerFrame &frame_i = it_per_id.feature_per_frame[frame_count - 2 - it_per_id.start_frame];
    const FeaturePerFrame &frame_j = it_per_id.feature_per_frame[frame_count - 1 - it_per_id.start_frame];

    double ans = 0;
    Vector3d p_j = frame_j.point;

    double u_j = p_j(0);
    double v_j = p_j(1);

    Vector3d p_i = frame_i.point;
    Vector3d p_i_comp;

    p_i_comp = p_i;
    double dep_i = p_i(2);
    double u_i = p_i(0) / dep_i;
    double v_i = p_i(1) / dep_i;
    double du = u_i - u_j, dv = v_i - v_j;

    double dep_i_comp = p_i_comp(2);
    double u_i_comp = p_i_comp(0) / dep_i_comp;
    double v_i_comp = p_i_comp(1) / dep_i_comp;
    double du_comp = u_i_comp - u_j, dv_comp = v_i_comp - v_j;

    ans = max(ans, sqrt(min(du * du + dv * dv, du_comp * du_comp + dv_comp * dv_comp)));

    return ans;
}


int
icatApplyRule( rsComm_t *rsComm, char *ruleName, char *arg1 ) {
    ruleExecInfo_t rei;
    int status;
    const char *args[2];

    rodsLog( LOG_DEBUG, "icatApplyRule called" );
    memset( ( char* )&rei, 0, sizeof( rei ) );
    args[0] = arg1;
    rei.rsComm = rsComm;
    rei.uoic = &rsComm->clientUser;
    rei.uoip = &rsComm->proxyUser;
    status =  applyRuleArg( ruleName,
                            args, 1, &rei, NO_SAVE_REI );
    return status;
}

#include "administration_utilities.hpp"
#include "userAdmin.h"
#include "rsUserAdmin.hpp"
#include "icatHighLevelRoutines.hpp"
#include "miscServerFunct.hpp"
#include "irods_configuration_keywords.hpp"

int
rsUserAdmin( rsComm_t *rsComm, userAdminInp_t *userAdminInp ) {
    rodsServerHost_t *rodsServerHost;
    int status;

    rodsLog( LOG_DEBUG, "userAdmin" );

    status = getAndConnRcatHost( rsComm, MASTER_RCAT, ( const char* )NULL, &rodsServerHost );
    if ( status < 0 ) {
        return status;
    }

    if ( rodsServerHost->localFlag == LOCAL_HOST ) {
        std::string svc_role;
        irods::error ret = get_catalog_service_role(svc_role);
        if(!ret.ok()) {
            irods::log(PASS(ret));
            return ret.code();
        }

        if( irods::CFG_SERVICE_ROLE_PROVIDER == svc_role ) {
            status = _rsUserAdmin( rsComm, userAdminInp );
        } else if( irods::CFG_SERVICE_ROLE_CONSUMER == svc_role ) {
            status = SYS_NO_RCAT_SERVER_ERR;
        } else {
            rodsLog(
                LOG_ERROR,
                "role not supported [%s]",
                svc_role.c_str() );
            status = SYS_SERVICE_ROLE_NOT_SUPPORTED;
        }

    }
    else {
        status = rcUserAdmin( rodsServerHost->conn,
                              userAdminInp );
    }

    if ( status < 0 ) {
        rodsLog( LOG_NOTICE,
                 "rsUserAdmin: rcUserAdmin failed" );
    }
    return status;
}

int
_rsUserAdmin( rsComm_t *rsComm, userAdminInp_t *userAdminInp ) {
    int status, status2;

    const char *args[MAX_NUM_OF_ARGS_IN_ACTION];

    int argc;
    ruleExecInfo_t rei2;

    memset( ( char* )&rei2, 0, sizeof( ruleExecInfo_t ) );
    rei2.rsComm = rsComm;
    if ( rsComm != NULL ) {
        rei2.uoic = &rsComm->clientUser;
        rei2.uoip = &rsComm->proxyUser;
    }

    rodsLog( LOG_DEBUG,
             "_rsUserAdmin arg0=%s",
             userAdminInp->arg0 );

    if ( strcmp( userAdminInp->arg0, "userpw" ) == 0 ) {
        args[0] = userAdminInp->arg1; /* username */
        args[1] = userAdminInp->arg2; /* option */
        args[2] = userAdminInp->arg3; /* newValue */
        argc = 3;
        status2 = applyRuleArg( "acPreProcForModifyUser",
                                args, argc, &rei2, NO_SAVE_REI );
        if ( status2 < 0 ) {
            if ( rei2.status < 0 ) {
                status2 = rei2.status;
            }
            rodsLog( LOG_ERROR, "_rsUserAdmin: acPreProcForModifyUser failed [%s] [%s] [%d]", userAdminInp->arg1, userAdminInp->arg2, status2 );
            return status2;
        }
        status = chlModUser( rsComm,
                             userAdminInp->arg1,
                             userAdminInp->arg2,
                             userAdminInp->arg3 );
        if ( status != 0 ) {
            chlRollback( rsComm );
        }

        status2 = applyRuleArg( "acPostProcForModifyUser", args, argc,
                                &rei2, NO_SAVE_REI );
        if ( status2 < 0 ) {
            if ( rei2.status < 0 ) {
                status2 = rei2.status;
            }
            rodsLog( LOG_ERROR, "rsUserAdmin:acPreProcForModifyUser error for %s and option %s,stat=%d",
                     userAdminInp->arg1, userAdminInp->arg2, status2 );
            return status2;
        }
        return status;
    }
    if ( strcmp( userAdminInp->arg0, "modify" ) == 0 ) {
        if ( strcmp( userAdminInp->arg1, "group" ) == 0 ) {
            args[0] = userAdminInp->arg2; /* groupname */
            args[1] = userAdminInp->arg3; /* option */
            args[2] = userAdminInp->arg4; /* username */
            args[3] = userAdminInp->arg5; /* zonename */
            argc = 4;
            status2 = applyRuleArg( "acPreProcForModifyUserGroup",
                                    args, argc, &rei2, NO_SAVE_REI );
            if ( status2 < 0 ) {
                if ( rei2.status < 0 ) {
                    status2 = rei2.status;
                }
                rodsLog( LOG_ERROR, "rsUserAdmin:acPreProcForModifyUserGroup error for %s and option %s,stat=%d", args[0], args[1], status2 );
                return status2;
            }

            status = chlModGroup( rsComm, userAdminInp->arg2,
                                  userAdminInp->arg3, userAdminInp->arg4,
                                  userAdminInp->arg5 );
            if ( status == 0 ) {
                status2 = applyRuleArg( "acPostProcForModifyUserGroup", args, argc,
                                        &rei2, NO_SAVE_REI );
                if ( status2 < 0 ) {
                    if ( rei2.status < 0 ) {
                        status2 = rei2.status;
                    }
                    rodsLog( LOG_ERROR, "rsUserAdmin:acPostProcForModifyUserGroup error for %s and option %s,stat=%d", args[0], args[1], status2 );
                    return status2;
                }
            }
            return status;
        }
    }
    if ( strcmp( userAdminInp->arg0, "mkuser" ) == 0 ) {
        return irods::create_user(*rsComm,
                                  userAdminInp->arg1 ? userAdminInp->arg1 : "",
                                  "rodsuser", "");
    }
    if ( strcmp( userAdminInp->arg0, "mkgroup" ) == 0 ) {
        return irods::create_user(*rsComm,
                                  userAdminInp->arg1 ? userAdminInp->arg1 : "",
                                  userAdminInp->arg2 ? userAdminInp->arg2 : "",
                                  "");
    }

    return CAT_INVALID_ARGUMENT;
}

/*
   This is the function used by chlModUser to run a rule to check a
   password.  It is in this source file instead of
   icatHighLevelRoutines.c so that test programs and define a dummy
   version of this and avoid linking with, and possibly trying to use,
   the Rule Engine.
*/