<commit_before>//===-- PPCCodeEmitter.cpp - JIT Code Emitter for PowerPC32 -------*- C++ -*-=//
//
//                     The LLVM Compiler Infrastructure
//
// This file was developed by the LLVM research group and is distributed under
// the University of Illinois Open Source License. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//
//
// This file defines the PowerPC 32-bit CodeEmitter and associated machinery to
// JIT-compile bytecode to native PowerPC.
//
//===----------------------------------------------------------------------===//

#include "PPCTargetMachine.h"
#include "PPCRelocations.h"
#include "PPC.h"
#include "llvm/Module.h"
#include "llvm/PassManager.h"
#include "llvm/CodeGen/MachineCodeEmitter.h"
#include "llvm/CodeGen/MachineFunctionPass.h"
#include "llvm/CodeGen/MachineInstrBuilder.h"
#include "llvm/CodeGen/Passes.h"
#include "llvm/Support/Debug.h"
#include "llvm/Target/TargetOptions.h"
#include <iostream>
using namespace llvm;

namespace {
  class PPCCodeEmitter : public MachineFunctionPass {
    TargetMachine &TM;
    MachineCodeEmitter &MCE;

    // Tracks which instruction references which BasicBlock
    std::vector<std::pair<MachineBasicBlock*, unsigned*> > BBRefs;
    // Tracks where each BasicBlock starts
    std::map<MachineBasicBlock*, long> BBLocations;

    /// getMachineOpValue - evaluates the MachineOperand of a given MachineInstr
    ///
    int getMachineOpValue(MachineInstr &MI, MachineOperand &MO);

  public:
    PPCCodeEmitter(TargetMachine &T, MachineCodeEmitter &M)
      : TM(T), MCE(M) {}

    const char *getPassName() const { return "PowerPC Machine Code Emitter"; }

    /// runOnMachineFunction - emits the given MachineFunction to memory
    ///
    bool runOnMachineFunction(MachineFunction &MF);

    /// emitBasicBlock - emits the given MachineBasicBlock to memory
    ///
    void emitBasicBlock(MachineBasicBlock &MBB);

    /// emitWord - write a 32-bit word to memory at the current PC
    ///
    void emitWord(unsigned w) { MCE.emitWord(w); }

    /// getValueBit - return the particular bit of Val
    ///
    unsigned getValueBit(int64_t Val, unsigned bit) { return (Val >> bit) & 1; }

    /// getBinaryCodeForInstr - This function, generated by the
    /// CodeEmitterGenerator using TableGen, produces the binary encoding for
    /// machine instructions.
    ///
    unsigned getBinaryCodeForInstr(MachineInstr &MI);
  };
}

/// addPassesToEmitMachineCode - Add passes to the specified pass manager to get
/// machine code emitted.  This uses a MachineCodeEmitter object to handle
/// actually outputting the machine code and resolving things like the address
/// of functions.  This method should returns true if machine code emission is
/// not supported.
///
bool PPCTargetMachine::addPassesToEmitMachineCode(FunctionPassManager &PM,
                                                  MachineCodeEmitter &MCE) {
  // Machine code emitter pass for PowerPC
  PM.add(new PPCCodeEmitter(*this, MCE));
  // Delete machine code for this function after emitting it
  PM.add(createMachineCodeDeleter());
  return false;
}

bool PPCCodeEmitter::runOnMachineFunction(MachineFunction &MF) {
  assert((MF.getTarget().getRelocationModel() != Reloc::Default ||
          MF.getTarget().getRelocationModel() != Reloc::Static) &&
         "JIT relocation model must be set to static or default!");
  MCE.startFunction(MF);
  MCE.emitConstantPool(MF.getConstantPool());
  for (MachineFunction::iterator BB = MF.begin(), E = MF.end(); BB != E; ++BB)
    emitBasicBlock(*BB);
  MCE.finishFunction(MF);

  // Resolve branches to BasicBlocks for the entire function
  for (unsigned i = 0, e = BBRefs.size(); i != e; ++i) {
    intptr_t Location = BBLocations[BBRefs[i].first];
    unsigned *Ref = BBRefs[i].second;
    DEBUG(std::cerr << "Fixup @ " << (void*)Ref << " to " << (void*)Location
                    << "\n");
    unsigned Instr = *Ref;
    intptr_t BranchTargetDisp = (Location - (intptr_t)Ref) >> 2;

    switch (Instr >> 26) {
    default: assert(0 && "Unknown branch user!");
    case 18:  // This is B or BL
      *Ref |= (BranchTargetDisp & ((1 << 24)-1)) << 2;
      break;
    case 16:  // This is BLT,BLE,BEQ,BGE,BGT,BNE, or other bcx instruction
      *Ref |= (BranchTargetDisp & ((1 << 14)-1)) << 2;
      break;
    }
  }
  BBRefs.clear();
  BBLocations.clear();

  return false;
}

void PPCCodeEmitter::emitBasicBlock(MachineBasicBlock &MBB) {
  BBLocations[&MBB] = MCE.getCurrentPCValue();
  for (MachineBasicBlock::iterator I = MBB.begin(), E = MBB.end(); I != E; ++I){
    MachineInstr &MI = *I;
    unsigned Opcode = MI.getOpcode();
    switch (MI.getOpcode()) {
    default:
      emitWord(getBinaryCodeForInstr(*I));
      break;
    case PPC::IMPLICIT_DEF_GPR:
    case PPC::IMPLICIT_DEF_F8:
    case PPC::IMPLICIT_DEF_F4:
      break; // pseudo opcode, no side effects
    case PPC::MovePCtoLR:
      assert(0 && "CodeEmitter does not support MovePCtoLR instruction");
      break;
    }
  }
}

static unsigned enumRegToMachineReg(unsigned enumReg) {
  switch (enumReg) {
  case PPC::R0 :  case PPC::F0 :  case PPC::V0 : case PPC::CR0:  return  0;
  case PPC::R1 :  case PPC::F1 :  case PPC::V1 : case PPC::CR1:  return  1;
  case PPC::R2 :  case PPC::F2 :  case PPC::V2 : case PPC::CR2:  return  2;
  case PPC::R3 :  case PPC::F3 :  case PPC::V3 : case PPC::CR3:  return  3;
  case PPC::R4 :  case PPC::F4 :  case PPC::V4 : case PPC::CR4:  return  4;
  case PPC::R5 :  case PPC::F5 :  case PPC::V5 : case PPC::CR5:  return  5;
  case PPC::R6 :  case PPC::F6 :  case PPC::V6 : case PPC::CR6:  return  6;
  case PPC::R7 :  case PPC::F7 :  case PPC::V7 : case PPC::CR7:  return  7;
  case PPC::R8 :  case PPC::F8 :  case PPC::V8 : return  8;
  case PPC::R9 :  case PPC::F9 :  case PPC::V9 : return  9;
  case PPC::R10:  case PPC::F10:  case PPC::V10: return 10;
  case PPC::R11:  case PPC::F11:  case PPC::V11: return 11;
  case PPC::R12:  case PPC::F12:  case PPC::V12: return 12;
  case PPC::R13:  case PPC::F13:  case PPC::V13: return 13;
  case PPC::R14:  case PPC::F14:  case PPC::V14: return 14;
  case PPC::R15:  case PPC::F15:  case PPC::V15: return 15;
  case PPC::R16:  case PPC::F16:  case PPC::V16: return 16;
  case PPC::R17:  case PPC::F17:  case PPC::V17: return 17;
  case PPC::R18:  case PPC::F18:  case PPC::V18: return 18;
  case PPC::R19:  case PPC::F19:  case PPC::V19: return 19;
  case PPC::R20:  case PPC::F20:  case PPC::V20: return 20;
  case PPC::R21:  case PPC::F21:  case PPC::V21: return 21;
  case PPC::R22:  case PPC::F22:  case PPC::V22: return 22;
  case PPC::R23:  case PPC::F23:  case PPC::V23: return 23;
  case PPC::R24:  case PPC::F24:  case PPC::V24: return 24;
  case PPC::R25:  case PPC::F25:  case PPC::V25: return 25;
  case PPC::R26:  case PPC::F26:  case PPC::V26: return 26;
  case PPC::R27:  case PPC::F27:  case PPC::V27: return 27;
  case PPC::R28:  case PPC::F28:  case PPC::V28: return 28;
  case PPC::R29:  case PPC::F29:  case PPC::V29: return 29;
  case PPC::R30:  case PPC::F30:  case PPC::V30: return 30;
  case PPC::R31:  case PPC::F31:  case PPC::V31: return 31;
  default:
    std::cerr << "Unhandled reg in enumRegToRealReg!\n";
    abort();
  }
}

int PPCCodeEmitter::getMachineOpValue(MachineInstr &MI, MachineOperand &MO) {

  int rv = 0; // Return value; defaults to 0 for unhandled cases
                  // or things that get fixed up later by the JIT.
  if (MO.isRegister()) {
    rv = enumRegToMachineReg(MO.getReg());

    // Special encoding for MTCRF and MFOCRF, which uses a bit mask for the
    // register, not the register number directly.
    if ((MI.getOpcode() == PPC::MTCRF || MI.getOpcode() == PPC::MFOCRF) &&
        (MO.getReg() >= PPC::CR0 && MO.getReg() <= PPC::CR7)) {
      rv = 0x80 >> rv;
    }
  } else if (MO.isImmediate()) {
    rv = MO.getImmedValue();
  } else if (MO.isGlobalAddress() || MO.isExternalSymbol()) {
    bool isExternal = MO.isExternalSymbol() ||
                      MO.getGlobal()->hasWeakLinkage() ||
                      MO.getGlobal()->hasLinkOnceLinkage() ||
                      (MO.getGlobal()->isExternal() &&
                       !MO.getGlobal()->hasNotBeenReadFromBytecode());
    unsigned Reloc = 0;
    if (MI.getOpcode() == PPC::BL)
      Reloc = PPC::reloc_pcrel_bx;
    else {
      switch (MI.getOpcode()) {
      default: MI.dump(); assert(0 && "Unknown instruction for relocation!");
      case PPC::LIS:
        if (isExternal)
          Reloc = PPC::reloc_absolute_ptr_high;   // Pointer to stub
        else
          Reloc = PPC::reloc_absolute_high;       // Pointer to symbol
        break;
      case PPC::LA:
        assert(!isExternal && "Something in the ISEL changed\n");
        Reloc = PPC::reloc_absolute_low;
        break;
      case PPC::LBZ:
      case PPC::LHA:
      case PPC::LHZ:
      case PPC::LWZ:
      case PPC::LFS:
      case PPC::LFD:
      case PPC::STB:
      case PPC::STH:
      case PPC::STW:
      case PPC::STFS:
      case PPC::STFD:
        if (isExternal)
          Reloc = PPC::reloc_absolute_ptr_low;
        else
          Reloc = PPC::reloc_absolute_low;
        break;
      }
    }
    if (MO.isGlobalAddress())
      MCE.addRelocation(MachineRelocation(MCE.getCurrentPCOffset(),
                                          Reloc, MO.getGlobal(), 0));
    else
      MCE.addRelocation(MachineRelocation(MCE.getCurrentPCOffset(),
                                          Reloc, MO.getSymbolName(), 0));
  } else if (MO.isMachineBasicBlock()) {
    unsigned* CurrPC = (unsigned*)(intptr_t)MCE.getCurrentPCValue();
    BBRefs.push_back(std::make_pair(MO.getMachineBasicBlock(), CurrPC));
  } else if (MO.isConstantPoolIndex()) {
    unsigned index = MO.getConstantPoolIndex();
    unsigned Opcode = MI.getOpcode();
    rv = MCE.getConstantPoolEntryAddress(index);
    if (Opcode == PPC::LIS || Opcode == PPC::ADDIS) {
      // lis wants hi16(addr)
      if ((short)rv < 0) rv += 1 << 16;
      rv >>= 16;
    } else if (Opcode == PPC::LWZ || Opcode == PPC::LA ||
               Opcode == PPC::LI ||
               Opcode == PPC::LFS || Opcode == PPC::LFD) {
      // These load opcodes want lo16(addr)
      rv &= 0xffff;
    } else {
      assert(0 && "Unknown constant pool using instruction!");
    }
  } else {
    std::cerr << "ERROR: Unknown type of MachineOperand: " << MO << "\n";
    abort();
  }

  return rv;
}

#include "PPCGenCodeEmitter.inc"

<commit_msg>Don't emit pseudo instructions!<commit_after>//===-- PPCCodeEmitter.cpp - JIT Code Emitter for PowerPC32 -------*- C++ -*-=//
//
//                     The LLVM Compiler Infrastructure
//
// This file was developed by the LLVM research group and is distributed under
// the University of Illinois Open Source License. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//
//
// This file defines the PowerPC 32-bit CodeEmitter and associated machinery to
// JIT-compile bytecode to native PowerPC.
//
//===----------------------------------------------------------------------===//

#include "PPCTargetMachine.h"
#include "PPCRelocations.h"
#include "PPC.h"
#include "llvm/Module.h"
#include "llvm/PassManager.h"
#include "llvm/CodeGen/MachineCodeEmitter.h"
#include "llvm/CodeGen/MachineFunctionPass.h"
#include "llvm/CodeGen/MachineInstrBuilder.h"
#include "llvm/CodeGen/Passes.h"
#include "llvm/Support/Debug.h"
#include "llvm/Target/TargetOptions.h"
#include <iostream>
using namespace llvm;

namespace {
  class PPCCodeEmitter : public MachineFunctionPass {
    TargetMachine &TM;
    MachineCodeEmitter &MCE;

    // Tracks which instruction references which BasicBlock
    std::vector<std::pair<MachineBasicBlock*, unsigned*> > BBRefs;
    // Tracks where each BasicBlock starts
    std::map<MachineBasicBlock*, long> BBLocations;

    /// getMachineOpValue - evaluates the MachineOperand of a given MachineInstr
    ///
    int getMachineOpValue(MachineInstr &MI, MachineOperand &MO);

  public:
    PPCCodeEmitter(TargetMachine &T, MachineCodeEmitter &M)
      : TM(T), MCE(M) {}

    const char *getPassName() const { return "PowerPC Machine Code Emitter"; }

    /// runOnMachineFunction - emits the given MachineFunction to memory
    ///
    bool runOnMachineFunction(MachineFunction &MF);

    /// emitBasicBlock - emits the given MachineBasicBlock to memory
    ///
    void emitBasicBlock(MachineBasicBlock &MBB);

    /// emitWord - write a 32-bit word to memory at the current PC
    ///
    void emitWord(unsigned w) { MCE.emitWord(w); }

    /// getValueBit - return the particular bit of Val
    ///
    unsigned getValueBit(int64_t Val, unsigned bit) { return (Val >> bit) & 1; }

    /// getBinaryCodeForInstr - This function, generated by the
    /// CodeEmitterGenerator using TableGen, produces the binary encoding for
    /// machine instructions.
    ///
    unsigned getBinaryCodeForInstr(MachineInstr &MI);
  };
}

/// addPassesToEmitMachineCode - Add passes to the specified pass manager to get
/// machine code emitted.  This uses a MachineCodeEmitter object to handle
/// actually outputting the machine code and resolving things like the address
/// of functions.  This method should returns true if machine code emission is
/// not supported.
///
bool PPCTargetMachine::addPassesToEmitMachineCode(FunctionPassManager &PM,
                                                  MachineCodeEmitter &MCE) {
  // Machine code emitter pass for PowerPC
  PM.add(new PPCCodeEmitter(*this, MCE));
  // Delete machine code for this function after emitting it
  PM.add(createMachineCodeDeleter());
  return false;
}

bool PPCCodeEmitter::runOnMachineFunction(MachineFunction &MF) {
  assert((MF.getTarget().getRelocationModel() != Reloc::Default ||
          MF.getTarget().getRelocationModel() != Reloc::Static) &&
         "JIT relocation model must be set to static or default!");
  MCE.startFunction(MF);
  MCE.emitConstantPool(MF.getConstantPool());
  for (MachineFunction::iterator BB = MF.begin(), E = MF.end(); BB != E; ++BB)
    emitBasicBlock(*BB);
  MCE.finishFunction(MF);

  // Resolve branches to BasicBlocks for the entire function
  for (unsigned i = 0, e = BBRefs.size(); i != e; ++i) {
    intptr_t Location = BBLocations[BBRefs[i].first];
    unsigned *Ref = BBRefs[i].second;
    DEBUG(std::cerr << "Fixup @ " << (void*)Ref << " to " << (void*)Location
                    << "\n");
    unsigned Instr = *Ref;
    intptr_t BranchTargetDisp = (Location - (intptr_t)Ref) >> 2;

    switch (Instr >> 26) {
    default: assert(0 && "Unknown branch user!");
    case 18:  // This is B or BL
      *Ref |= (BranchTargetDisp & ((1 << 24)-1)) << 2;
      break;
    case 16:  // This is BLT,BLE,BEQ,BGE,BGT,BNE, or other bcx instruction
      *Ref |= (BranchTargetDisp & ((1 << 14)-1)) << 2;
      break;
    }
  }
  BBRefs.clear();
  BBLocations.clear();

  return false;
}

void PPCCodeEmitter::emitBasicBlock(MachineBasicBlock &MBB) {
  BBLocations[&MBB] = MCE.getCurrentPCValue();
  for (MachineBasicBlock::iterator I = MBB.begin(), E = MBB.end(); I != E; ++I){
    MachineInstr &MI = *I;
    unsigned Opcode = MI.getOpcode();
    switch (MI.getOpcode()) {
    default:
      emitWord(getBinaryCodeForInstr(*I));
      break;
    case PPC::IMPLICIT_DEF_GPR:
    case PPC::IMPLICIT_DEF_F8:
    case PPC::IMPLICIT_DEF_F4:
    case PPC::IMPLICIT_DEF_VRRC:
      break; // pseudo opcode, no side effects
    case PPC::MovePCtoLR:
      assert(0 && "CodeEmitter does not support MovePCtoLR instruction");
      break;
    }
  }
}

static unsigned enumRegToMachineReg(unsigned enumReg) {
  switch (enumReg) {
  case PPC::R0 :  case PPC::F0 :  case PPC::V0 : case PPC::CR0:  return  0;
  case PPC::R1 :  case PPC::F1 :  case PPC::V1 : case PPC::CR1:  return  1;
  case PPC::R2 :  case PPC::F2 :  case PPC::V2 : case PPC::CR2:  return  2;
  case PPC::R3 :  case PPC::F3 :  case PPC::V3 : case PPC::CR3:  return  3;
  case PPC::R4 :  case PPC::F4 :  case PPC::V4 : case PPC::CR4:  return  4;
  case PPC::R5 :  case PPC::F5 :  case PPC::V5 : case PPC::CR5:  return  5;
  case PPC::R6 :  case PPC::F6 :  case PPC::V6 : case PPC::CR6:  return  6;
  case PPC::R7 :  case PPC::F7 :  case PPC::V7 : case PPC::CR7:  return  7;
  case PPC::R8 :  case PPC::F8 :  case PPC::V8 : return  8;
  case PPC::R9 :  case PPC::F9 :  case PPC::V9 : return  9;
  case PPC::R10:  case PPC::F10:  case PPC::V10: return 10;
  case PPC::R11:  case PPC::F11:  case PPC::V11: return 11;
  case PPC::R12:  case PPC::F12:  case PPC::V12: return 12;
  case PPC::R13:  case PPC::F13:  case PPC::V13: return 13;
  case PPC::R14:  case PPC::F14:  case PPC::V14: return 14;
  case PPC::R15:  case PPC::F15:  case PPC::V15: return 15;
  case PPC::R16:  case PPC::F16:  case PPC::V16: return 16;
  case PPC::R17:  case PPC::F17:  case PPC::V17: return 17;
  case PPC::R18:  case PPC::F18:  case PPC::V18: return 18;
  case PPC::R19:  case PPC::F19:  case PPC::V19: return 19;
  case PPC::R20:  case PPC::F20:  case PPC::V20: return 20;
  case PPC::R21:  case PPC::F21:  case PPC::V21: return 21;
  case PPC::R22:  case PPC::F22:  case PPC::V22: return 22;
  case PPC::R23:  case PPC::F23:  case PPC::V23: return 23;
  case PPC::R24:  case PPC::F24:  case PPC::V24: return 24;
  case PPC::R25:  case PPC::F25:  case PPC::V25: return 25;
  case PPC::R26:  case PPC::F26:  case PPC::V26: return 26;
  case PPC::R27:  case PPC::F27:  case PPC::V27: return 27;
  case PPC::R28:  case PPC::F28:  case PPC::V28: return 28;
  case PPC::R29:  case PPC::F29:  case PPC::V29: return 29;
  case PPC::R30:  case PPC::F30:  case PPC::V30: return 30;
  case PPC::R31:  case PPC::F31:  case PPC::V31: return 31;
  default:
    std::cerr << "Unhandled reg in enumRegToRealReg!\n";
    abort();
  }
}

int PPCCodeEmitter::getMachineOpValue(MachineInstr &MI, MachineOperand &MO) {

  int rv = 0; // Return value; defaults to 0 for unhandled cases
                  // or things that get fixed up later by the JIT.
  if (MO.isRegister()) {
    rv = enumRegToMachineReg(MO.getReg());

    // Special encoding for MTCRF and MFOCRF, which uses a bit mask for the
    // register, not the register number directly.
    if ((MI.getOpcode() == PPC::MTCRF || MI.getOpcode() == PPC::MFOCRF) &&
        (MO.getReg() >= PPC::CR0 && MO.getReg() <= PPC::CR7)) {
      rv = 0x80 >> rv;
    }
  } else if (MO.isImmediate()) {
    rv = MO.getImmedValue();
  } else if (MO.isGlobalAddress() || MO.isExternalSymbol()) {
    bool isExternal = MO.isExternalSymbol() ||
                      MO.getGlobal()->hasWeakLinkage() ||
                      MO.getGlobal()->hasLinkOnceLinkage() ||
                      (MO.getGlobal()->isExternal() &&
                       !MO.getGlobal()->hasNotBeenReadFromBytecode());
    unsigned Reloc = 0;
    if (MI.getOpcode() == PPC::BL)
      Reloc = PPC::reloc_pcrel_bx;
    else {
      switch (MI.getOpcode()) {
      default: MI.dump(); assert(0 && "Unknown instruction for relocation!");
      case PPC::LIS:
        if (isExternal)
          Reloc = PPC::reloc_absolute_ptr_high;   // Pointer to stub
        else
          Reloc = PPC::reloc_absolute_high;       // Pointer to symbol
        break;
      case PPC::LA:
        assert(!isExternal && "Something in the ISEL changed\n");
        Reloc = PPC::reloc_absolute_low;
        break;
      case PPC::LBZ:
      case PPC::LHA:
      case PPC::LHZ:
      case PPC::LWZ:
      case PPC::LFS:
      case PPC::LFD:
      case PPC::STB:
      case PPC::STH:
      case PPC::STW:
      case PPC::STFS:
      case PPC::STFD:
        if (isExternal)
          Reloc = PPC::reloc_absolute_ptr_low;
        else
          Reloc = PPC::reloc_absolute_low;
        break;
      }
    }
    if (MO.isGlobalAddress())
      MCE.addRelocation(MachineRelocation(MCE.getCurrentPCOffset(),
                                          Reloc, MO.getGlobal(), 0));
    else
      MCE.addRelocation(MachineRelocation(MCE.getCurrentPCOffset(),
                                          Reloc, MO.getSymbolName(), 0));
  } else if (MO.isMachineBasicBlock()) {
    unsigned* CurrPC = (unsigned*)(intptr_t)MCE.getCurrentPCValue();
    BBRefs.push_back(std::make_pair(MO.getMachineBasicBlock(), CurrPC));
  } else if (MO.isConstantPoolIndex()) {
    unsigned index = MO.getConstantPoolIndex();
    unsigned Opcode = MI.getOpcode();
    rv = MCE.getConstantPoolEntryAddress(index);
    if (Opcode == PPC::LIS || Opcode == PPC::ADDIS) {
      // lis wants hi16(addr)
      if ((short)rv < 0) rv += 1 << 16;
      rv >>= 16;
    } else if (Opcode == PPC::LWZ || Opcode == PPC::LA ||
               Opcode == PPC::LI ||
               Opcode == PPC::LFS || Opcode == PPC::LFD) {
      // These load opcodes want lo16(addr)
      rv &= 0xffff;
    } else {
      assert(0 && "Unknown constant pool using instruction!");
    }
  } else {
    std::cerr << "ERROR: Unknown type of MachineOperand: " << MO << "\n";
    abort();
  }

  return rv;
}

#include "PPCGenCodeEmitter.inc"

<|endoftext|>


<commit_before>/*
 * Copyright (c) 2008, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include "robot.h"
#include "robot_link.h"
#include "common.h"
#include "properties/property.h"
#include "properties/property_manager.h"
#include "visualization_manager.h"

#include "ogre_tools/object.h"
#include "ogre_tools/shape.h"
#include "ogre_tools/axes.h"

#include <urdf/model.h>

#include <OGRE/OgreSceneNode.h>
#include <OGRE/OgreSceneManager.h>
#include <OGRE/OgreRibbonTrail.h>
#include <OGRE/OgreEntity.h>
#include <OGRE/OgreMaterialManager.h>
#include <OGRE/OgreMaterial.h>
#include <OGRE/OgreResourceGroupManager.h>

#include <ros/console.h>

namespace rviz
{

Robot::Robot( VisualizationManager* manager, const std::string& name )
: scene_manager_(manager->getSceneManager())
, visual_visible_( true )
, collision_visible_( false )
, vis_manager_(manager)
, property_manager_(NULL)
, name_( name )
{
  root_visual_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
  root_collision_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
  root_other_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();

  setVisualVisible( visual_visible_ );
  setCollisionVisible( collision_visible_ );
  setAlpha(1.0f);
}

Robot::~Robot()
{
  clear();

  scene_manager_->destroySceneNode( root_visual_node_->getName() );
  scene_manager_->destroySceneNode( root_collision_node_->getName() );
  scene_manager_->destroySceneNode( root_other_node_->getName() );
}

void Robot::setVisible( bool visible )
{
  if ( visible )
  {
    root_visual_node_->setVisible( visual_visible_ );
    root_collision_node_->setVisible( collision_visible_ );
  }
  else
  {
    root_visual_node_->setVisible( false );
    root_collision_node_->setVisible( false );
  }
}

void Robot::setVisualVisible( bool visible )
{
  visual_visible_ = visible;
  root_visual_node_->setVisible( visible );
}

void Robot::setCollisionVisible( bool visible )
{
  collision_visible_ = visible;
  root_collision_node_->setVisible( visible );
}

bool Robot::isVisualVisible()
{
  return visual_visible_;
}

bool Robot::isCollisionVisible()
{
  return collision_visible_;
}

void Robot::setAlpha(float a)
{
  alpha_ = a;

  M_NameToLink::iterator it = links_.begin();
  M_NameToLink::iterator end = links_.end();
  for ( ; it != end; ++it )
  {
    RobotLink* info = it->second;

    info->setAlpha(alpha_);
  }
}

void Robot::clear()
{
  M_NameToLink::iterator link_it = links_.begin();
  M_NameToLink::iterator link_end = links_.end();
  for ( ; link_it != link_end; ++link_it )
  {
    RobotLink* info = link_it->second;
    delete info;
  }

  if ( property_manager_ )
  {
    property_manager_->deleteByUserData( this );
  }

  links_category_.reset();
  links_.clear();
  root_visual_node_->removeAndDestroyAllChildren();
  root_collision_node_->removeAndDestroyAllChildren();
}

void Robot::setPropertyManager( PropertyManager* property_manager, const CategoryPropertyWPtr& parent )
{
  ROS_ASSERT( property_manager );
  ROS_ASSERT( parent.lock() );

  property_manager_ = property_manager;
  parent_property_ = parent;

  links_category_ = property_manager_->createCategory( "Links", name_, parent_property_, this );

  if ( !links_.empty() )
  {
    M_NameToLink::iterator link_it = links_.begin();
    M_NameToLink::iterator link_end = links_.end();
    for ( ; link_it != link_end; ++link_it )
    {
      RobotLink* info = link_it->second;

      info->setPropertyManager(property_manager);
      info->createProperties();
    }
  }

  CategoryPropertyPtr cat_prop = links_category_.lock();
  cat_prop->collapse();
}

void Robot::load( TiXmlElement* root_element, urdf::Model &descr, bool visual, bool collision )
{
  clear();

  if ( property_manager_ )
  {
    ROS_ASSERT(!links_category_.lock());
    links_category_ = property_manager_->createCategory( "Links", name_, parent_property_, this );
  }

  typedef std::vector<boost::shared_ptr<urdf::Link> > V_Link;
  V_Link links;
  descr.getLinks(links);
  V_Link::iterator it = links.begin();
  V_Link::iterator end = links.end();
  for (; it != end; ++it)
  {
    const boost::shared_ptr<urdf::Link>& link = *it;

    RobotLink* link_info = new RobotLink(this, vis_manager_);
    if (property_manager_)
    {
      link_info->setPropertyManager(property_manager_);
    }
    link_info->load(root_element, descr, link, visual, collision);

    bool inserted = links_.insert( std::make_pair( link_info->getName(), link_info ) ).second;
    ROS_ASSERT( inserted );

    link_info->setAlpha(alpha_);
  }

  if (property_manager_)
  {
    CategoryPropertyPtr cat_prop = links_category_.lock();
    cat_prop->collapse();
  }

  setVisualVisible(isVisualVisible());
  setCollisionVisible(isCollisionVisible());
}

RobotLink* Robot::getLink( const std::string& name )
{
  M_NameToLink::iterator it = links_.find( name );
  if ( it == links_.end() )
  {
    ROS_WARN( "Link [%s] does not exist", name.c_str() );
    return NULL;
  }

  return it->second;
}

void Robot::update(const LinkUpdater& updater)
{
  M_NameToLink::iterator link_it = links_.begin();
  M_NameToLink::iterator link_end = links_.end();
  for ( ; link_it != link_end; ++link_it )
  {
    RobotLink* info = link_it->second;

    info->setToNormalMaterial();

    Ogre::Vector3 visual_position, collision_position;
    Ogre::Quaternion visual_orientation, collision_orientation;
    bool apply_offset_transforms;
    if (updater.getLinkTransforms(info->getName(), visual_position, visual_orientation, collision_position, collision_orientation, apply_offset_transforms))
    {
      info->setTransforms( visual_position, visual_orientation, collision_position, collision_orientation, apply_offset_transforms );
    }
    else
    {
      info->setToErrorMaterial();
    }
  }
}

void Robot::setPosition( const Ogre::Vector3& position )
{
  root_visual_node_->setPosition( position );
  root_collision_node_->setPosition( position );
}

void Robot::setOrientation( const Ogre::Quaternion& orientation )
{
  root_visual_node_->setOrientation( orientation );
  root_collision_node_->setOrientation( orientation );
}

void Robot::setScale( const Ogre::Vector3& scale )
{
  root_visual_node_->setScale( scale );
  root_collision_node_->setScale( scale );
}

const Ogre::Vector3& Robot::getPosition()
{
  return root_visual_node_->getPosition();
}

const Ogre::Quaternion& Robot::getOrientation()
{
  return root_visual_node_->getOrientation();
}

} // namespace rviz
<commit_msg>Sort robot frames by name<commit_after>/*
 * Copyright (c) 2008, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include "robot.h"
#include "robot_link.h"
#include "common.h"
#include "properties/property.h"
#include "properties/property_manager.h"
#include "visualization_manager.h"

#include "ogre_tools/object.h"
#include "ogre_tools/shape.h"
#include "ogre_tools/axes.h"

#include <urdf/model.h>

#include <OGRE/OgreSceneNode.h>
#include <OGRE/OgreSceneManager.h>
#include <OGRE/OgreRibbonTrail.h>
#include <OGRE/OgreEntity.h>
#include <OGRE/OgreMaterialManager.h>
#include <OGRE/OgreMaterial.h>
#include <OGRE/OgreResourceGroupManager.h>

#include <ros/console.h>

namespace rviz
{

Robot::Robot( VisualizationManager* manager, const std::string& name )
: scene_manager_(manager->getSceneManager())
, visual_visible_( true )
, collision_visible_( false )
, vis_manager_(manager)
, property_manager_(NULL)
, name_( name )
{
  root_visual_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
  root_collision_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
  root_other_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();

  setVisualVisible( visual_visible_ );
  setCollisionVisible( collision_visible_ );
  setAlpha(1.0f);
}

Robot::~Robot()
{
  clear();

  scene_manager_->destroySceneNode( root_visual_node_->getName() );
  scene_manager_->destroySceneNode( root_collision_node_->getName() );
  scene_manager_->destroySceneNode( root_other_node_->getName() );
}

void Robot::setVisible( bool visible )
{
  if ( visible )
  {
    root_visual_node_->setVisible( visual_visible_ );
    root_collision_node_->setVisible( collision_visible_ );
  }
  else
  {
    root_visual_node_->setVisible( false );
    root_collision_node_->setVisible( false );
  }
}

void Robot::setVisualVisible( bool visible )
{
  visual_visible_ = visible;
  root_visual_node_->setVisible( visible );
}

void Robot::setCollisionVisible( bool visible )
{
  collision_visible_ = visible;
  root_collision_node_->setVisible( visible );
}

bool Robot::isVisualVisible()
{
  return visual_visible_;
}

bool Robot::isCollisionVisible()
{
  return collision_visible_;
}

void Robot::setAlpha(float a)
{
  alpha_ = a;

  M_NameToLink::iterator it = links_.begin();
  M_NameToLink::iterator end = links_.end();
  for ( ; it != end; ++it )
  {
    RobotLink* info = it->second;

    info->setAlpha(alpha_);
  }
}

void Robot::clear()
{
  M_NameToLink::iterator link_it = links_.begin();
  M_NameToLink::iterator link_end = links_.end();
  for ( ; link_it != link_end; ++link_it )
  {
    RobotLink* info = link_it->second;
    delete info;
  }

  if ( property_manager_ )
  {
    property_manager_->deleteByUserData( this );
  }

  links_category_.reset();
  links_.clear();
  root_visual_node_->removeAndDestroyAllChildren();
  root_collision_node_->removeAndDestroyAllChildren();
}

void Robot::setPropertyManager( PropertyManager* property_manager, const CategoryPropertyWPtr& parent )
{
  ROS_ASSERT( property_manager );
  ROS_ASSERT( parent.lock() );

  property_manager_ = property_manager;
  parent_property_ = parent;

  links_category_ = property_manager_->createCategory( "Links", name_, parent_property_, this );

  if ( !links_.empty() )
  {
    M_NameToLink::iterator link_it = links_.begin();
    M_NameToLink::iterator link_end = links_.end();
    for ( ; link_it != link_end; ++link_it )
    {
      RobotLink* info = link_it->second;

      info->setPropertyManager(property_manager);
      info->createProperties();
    }
  }

  CategoryPropertyPtr cat_prop = links_category_.lock();
  cat_prop->collapse();
}

class LinkComparator
{
public:
  bool operator()(const boost::shared_ptr<urdf::Link>& lhs, const boost::shared_ptr<urdf::Link>& rhs)
  {
    return lhs->name < rhs->name;
  }
};

void Robot::load( TiXmlElement* root_element, urdf::Model &descr, bool visual, bool collision )
{
  clear();

  if ( property_manager_ )
  {
    ROS_ASSERT(!links_category_.lock());
    links_category_ = property_manager_->createCategory( "Links", name_, parent_property_, this );
  }

  typedef std::vector<boost::shared_ptr<urdf::Link> > V_Link;
  V_Link links;
  descr.getLinks(links);
  std::sort(links.begin(), links.end(), LinkComparator());
  V_Link::iterator it = links.begin();
  V_Link::iterator end = links.end();
  for (; it != end; ++it)
  {
    const boost::shared_ptr<urdf::Link>& link = *it;

    RobotLink* link_info = new RobotLink(this, vis_manager_);
    if (property_manager_)
    {
      link_info->setPropertyManager(property_manager_);
    }
    link_info->load(root_element, descr, link, visual, collision);

    bool inserted = links_.insert( std::make_pair( link_info->getName(), link_info ) ).second;
    ROS_ASSERT( inserted );

    link_info->setAlpha(alpha_);
  }

  if (property_manager_)
  {
    CategoryPropertyPtr cat_prop = links_category_.lock();
    cat_prop->collapse();
  }

  setVisualVisible(isVisualVisible());
  setCollisionVisible(isCollisionVisible());
}

RobotLink* Robot::getLink( const std::string& name )
{
  M_NameToLink::iterator it = links_.find( name );
  if ( it == links_.end() )
  {
    ROS_WARN( "Link [%s] does not exist", name.c_str() );
    return NULL;
  }

  return it->second;
}

void Robot::update(const LinkUpdater& updater)
{
  M_NameToLink::iterator link_it = links_.begin();
  M_NameToLink::iterator link_end = links_.end();
  for ( ; link_it != link_end; ++link_it )
  {
    RobotLink* info = link_it->second;

    info->setToNormalMaterial();

    Ogre::Vector3 visual_position, collision_position;
    Ogre::Quaternion visual_orientation, collision_orientation;
    bool apply_offset_transforms;
    if (updater.getLinkTransforms(info->getName(), visual_position, visual_orientation, collision_position, collision_orientation, apply_offset_transforms))
    {
      info->setTransforms( visual_position, visual_orientation, collision_position, collision_orientation, apply_offset_transforms );
    }
    else
    {
      info->setToErrorMaterial();
    }
  }
}

void Robot::setPosition( const Ogre::Vector3& position )
{
  root_visual_node_->setPosition( position );
  root_collision_node_->setPosition( position );
}

void Robot::setOrientation( const Ogre::Quaternion& orientation )
{
  root_visual_node_->setOrientation( orientation );
  root_collision_node_->setOrientation( orientation );
}

void Robot::setScale( const Ogre::Vector3& scale )
{
  root_visual_node_->setScale( scale );
  root_collision_node_->setScale( scale );
}

const Ogre::Vector3& Robot::getPosition()
{
  return root_visual_node_->getPosition();
}

const Ogre::Quaternion& Robot::getOrientation()
{
  return root_visual_node_->getOrientation();
}

} // namespace rviz
<|endoftext|>


<commit_before>// Jubatus: Online machine learning framework for distributed environment
// Copyright (C) 2011,2012 Preferred Infrastructure and Nippon Telegraph and Telephone Corporation.
//
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License version 2.1 as published by the Free Software Foundation.
//
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA

#include "stat_serv.hpp"

#include <pficommon/text/json.h>

#include "../common/shared_ptr.hpp"
#include "../framework/mixer/mixer_factory.hpp"

using namespace std;
using namespace jubatus::common;
using namespace jubatus::framework;
using pfi::text::json::json;
using pfi::text::json::json_cast;
using pfi::lang::lexical_cast;

namespace jubatus {
namespace server {

struct stat_serv_config {
  int32_t window_size;

  template <typename Ar>
  void serialize(Ar& ar) {
    ar & MEMBER(window_size);
  }
};

stat_serv::stat_serv(const server_argv& a,
                     const cshared_ptr<lock_service>& zk)
    : server_base(a) {
  stat_serv_config conf = { 1024 };
  config_ = lexical_cast<string>(pfi::text::json::to_json(conf));
  common::cshared_ptr<stat::mixable_stat> model(new stat::mixable_stat(conf.window_size));
  stat_.set_model(model);

  mixer_.reset(mixer::create_mixer(a, zk));
  mixable_holder_.reset(new mixable_holder());

  mixer_->set_mixable_holder(mixable_holder_);
  mixable_holder_->register_mixable(&stat_);
}

stat_serv::~stat_serv() {
}

mixer::mixer* stat_serv::get_mixer() const {
  return mixer_.get();
}

pfi::lang::shared_ptr<mixable_holder> stat_serv::get_mixable_holder() const {
  return mixable_holder_;
}

void stat_serv::get_status(status_t& status) const {
  status.insert(make_pair("storage", stat_.get_model()->type()));
}

bool stat_serv::set_config(const string& config) {
  LOG(INFO) << __func__;
  // TODO: error handling
  stat_serv_config conf = json_cast<stat_serv_config>(lexical_cast<json>(config));
  common::cshared_ptr<stat::mixable_stat> model(new stat::mixable_stat(conf.window_size));
  config_ = config;
  stat_.set_model(model);
  return 0;
}

string stat_serv::get_config() const {
  return config_;
}

int stat_serv::push(const std::string& key, double value) {
  stat_.get_model()->push(key,value);
  return 0;
}

double stat_serv::sum(const std::string& key) const {
  return stat_.get_model()->sum(key);
}

double stat_serv::stddev(const std::string& key) const {
  return stat_.get_model()->stddev(key);
}

double stat_serv::max(const std::string& key) const {
  return stat_.get_model()->max(key);
}

double stat_serv::min(const std::string& key) const {
  return stat_.get_model()->min(key);
}

double stat_serv::entropy(const std::string& key) const {
#ifdef HAVE_ZOOKEEPER_H
  //FIXME: currently gets old value of entropy when mix completed
  return stat_.get_model()->mixed_entropy();
#else
  return stat_.get_model()->entropy();
#endif
}

double stat_serv::moment(const std::string& key, int n,double c) const {
  return stat_.get_model()->moment(key, n, c);
}

} // namespace server
} // namespace jubatus
<commit_msg>Fix stat_serv based on jsonconfig<commit_after>// Jubatus: Online machine learning framework for distributed environment
// Copyright (C) 2011,2012 Preferred Infrastructure and Nippon Telegraph and Telephone Corporation.
//
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License version 2.1 as published by the Free Software Foundation.
//
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA

#include "stat_serv.hpp"

#include <pficommon/text/json.h>

#include "../common/shared_ptr.hpp"
#include "../common/jsonconfig.hpp"
#include "../framework/mixer/mixer_factory.hpp"

using namespace std;
using namespace jubatus::common;
using namespace jubatus::framework;
using pfi::text::json::json;
using pfi::text::json::json_cast;
using pfi::lang::lexical_cast;

namespace jubatus {
namespace server {

struct stat_serv_config {
  int32_t window_size;

  template <typename Ar>
  void serialize(Ar& ar) {
    ar & MEMBER(window_size);
  }
};

stat_serv::stat_serv(const server_argv& a,
                     const cshared_ptr<lock_service>& zk)
    : server_base(a) {
  stat_serv_config conf = { 1024 };
  config_ = lexical_cast<string>(pfi::text::json::to_json(conf));
  common::cshared_ptr<stat::mixable_stat> model(new stat::mixable_stat(conf.window_size));
  stat_.set_model(model);

  mixer_.reset(mixer::create_mixer(a, zk));
  mixable_holder_.reset(new mixable_holder());

  mixer_->set_mixable_holder(mixable_holder_);
  mixable_holder_->register_mixable(&stat_);
}

stat_serv::~stat_serv() {
}

mixer::mixer* stat_serv::get_mixer() const {
  return mixer_.get();
}

pfi::lang::shared_ptr<mixable_holder> stat_serv::get_mixable_holder() const {
  return mixable_holder_;
}

void stat_serv::get_status(status_t& status) const {
  status.insert(make_pair("storage", stat_.get_model()->type()));
}

bool stat_serv::set_config(const string& config) {
  LOG(INFO) << __func__;

  jsonconfig::config conf_root(lexical_cast<json>(config));
  stat_serv_config conf = jsonconfig::config_cast_check<stat_serv_config>(conf_root);

  common::cshared_ptr<stat::mixable_stat> model(new stat::mixable_stat(conf.window_size));
  config_ = config;
  stat_.set_model(model);
  return 0;
}

string stat_serv::get_config() const {
  return config_;
}

int stat_serv::push(const std::string& key, double value) {
  stat_.get_model()->push(key,value);
  return 0;
}

double stat_serv::sum(const std::string& key) const {
  return stat_.get_model()->sum(key);
}

double stat_serv::stddev(const std::string& key) const {
  return stat_.get_model()->stddev(key);
}

double stat_serv::max(const std::string& key) const {
  return stat_.get_model()->max(key);
}

double stat_serv::min(const std::string& key) const {
  return stat_.get_model()->min(key);
}

double stat_serv::entropy(const std::string& key) const {
#ifdef HAVE_ZOOKEEPER_H
  //FIXME: currently gets old value of entropy when mix completed
  return stat_.get_model()->mixed_entropy();
#else
  return stat_.get_model()->entropy();
#endif
}

double stat_serv::moment(const std::string& key, int n,double c) const {
  return stat_.get_model()->moment(key, n, c);
}

} // namespace server
} // namespace jubatus
<|endoftext|>


<commit_before>/*
 * Copyright (C) 2015 deipi.com LLC and contributors. All rights reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to
 * deal in the Software without restriction, including without limitation the
 * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
 * sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 * IN THE SOFTWARE.
 */

#include "discovery.h"

#ifdef XAPIAND_CLUSTERING

#include <assert.h>

constexpr const char* const Discovery::MessageNames[];

Discovery::Discovery(const std::shared_ptr<XapiandManager>& manager_, ev::loop_ref *loop_, int port_, const std::string &group_)
	: BaseUDP(manager_, loop_, port_, "Discovery", XAPIAND_DISCOVERY_PROTOCOL_VERSION, group_),
	  heartbeat(*loop)
{
	heartbeat.set<Discovery, &Discovery::heartbeat_cb>(this);
	heartbeat.repeat = random_real(HEARTBEAT_MIN, HEARTBEAT_MAX);
	L_DISCOVERY(this, "\tSet heartbeat timeout event %f", heartbeat.repeat);

	L_OBJ(this, "CREATED DISCOVERY");
}

Discovery::~Discovery()
{
	heartbeat.stop();

	L_OBJ(this, "DELETED DISCOVERY");
}

void
Discovery::heartbeat_cb(ev::timer &, int)
{
	L_EV_BEGIN(this, "Discovery::heartbeat_cb:BEGIN");

	auto m = manager();

	switch (m->state) {
		case XapiandManager::State::RESET:
			if (!local_node.name.empty()) {
				m->drop_node(local_node.name);
			}
			if (m->node_name.empty()) {
				local_node.name = name_generator();
			} else {
				local_node.name = m->node_name;
			}
			L_INFO(this, "Advertising as %s (id: %016llX)...", local_node.name.c_str(), local_node.id);
			send_message(Message::HELLO, local_node.serialise());
			m->state = XapiandManager::State::WAITING;
			break;

		case XapiandManager::State::WAITING:
			m->state = XapiandManager::State::WAITING_;
			break;

		case XapiandManager::State::WAITING_:
			m->state = XapiandManager::State::SETUP;
			break;

		case XapiandManager::State::SETUP:
			m->setup_node();
			break;

		case XapiandManager::State::READY:
			send_message(Message::HEARTBEAT, local_node.serialise());
			break;

		case XapiandManager::State::BAD:
			L_ERR(this, "ERROR: Manager is in BAD state!!");
			break;
	}
	L_EV_END(this, "Discovery::heartbeat_cb:END");
}

std::string
Discovery::getDescription() const noexcept
{
	return "UDP:" + std::to_string(port) + " (" + description + " v" + std::to_string(XAPIAND_DISCOVERY_PROTOCOL_MAJOR_VERSION) + "." + std::to_string(XAPIAND_DISCOVERY_PROTOCOL_MINOR_VERSION) + ")";
}

#endif
<commit_msg>Log manager not ready messages<commit_after>/*
 * Copyright (C) 2015 deipi.com LLC and contributors. All rights reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to
 * deal in the Software without restriction, including without limitation the
 * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
 * sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 * IN THE SOFTWARE.
 */

#include "discovery.h"

#ifdef XAPIAND_CLUSTERING

#include <assert.h>

constexpr const char* const Discovery::MessageNames[];

Discovery::Discovery(const std::shared_ptr<XapiandManager>& manager_, ev::loop_ref *loop_, int port_, const std::string &group_)
	: BaseUDP(manager_, loop_, port_, "Discovery", XAPIAND_DISCOVERY_PROTOCOL_VERSION, group_),
	  heartbeat(*loop)
{
	heartbeat.set<Discovery, &Discovery::heartbeat_cb>(this);
	heartbeat.repeat = random_real(HEARTBEAT_MIN, HEARTBEAT_MAX);
	L_DISCOVERY(this, "\tSet heartbeat timeout event %f", heartbeat.repeat);

	L_OBJ(this, "CREATED DISCOVERY");
}

Discovery::~Discovery()
{
	heartbeat.stop();

	L_OBJ(this, "DELETED DISCOVERY");
}

void
Discovery::heartbeat_cb(ev::timer &, int)
{
	L_EV_BEGIN(this, "Discovery::heartbeat_cb:BEGIN");

	auto m = manager();

	if (m->state != XapiandManager::State::READY) {
		L_DISCOVERY(this, "Waiting manager get ready!! (%s)", XapiandManager::StateNames[static_cast<int>(m->state)]);
	}

	switch (m->state) {
		case XapiandManager::State::RESET:
			if (!local_node.name.empty()) {
				m->drop_node(local_node.name);
			}
			if (m->node_name.empty()) {
				local_node.name = name_generator();
			} else {
				local_node.name = m->node_name;
			}
			L_INFO(this, "Advertising as %s (id: %016llX)...", local_node.name.c_str(), local_node.id);
			send_message(Message::HELLO, local_node.serialise());
			m->state = XapiandManager::State::WAITING;
			break;

		case XapiandManager::State::WAITING:
			m->state = XapiandManager::State::WAITING_;
			break;

		case XapiandManager::State::WAITING_:
			m->state = XapiandManager::State::SETUP;
			break;

		case XapiandManager::State::SETUP:
			m->setup_node();
			break;

		case XapiandManager::State::READY:
			send_message(Message::HEARTBEAT, local_node.serialise());
			break;

		case XapiandManager::State::BAD:
			L_ERR(this, "ERROR: Manager is in BAD state!!");
			break;
	}

	L_EV_END(this, "Discovery::heartbeat_cb:END");
}

std::string
Discovery::getDescription() const noexcept
{
	return "UDP:" + std::to_string(port) + " (" + description + " v" + std::to_string(XAPIAND_DISCOVERY_PROTOCOL_MAJOR_VERSION) + "." + std::to_string(XAPIAND_DISCOVERY_PROTOCOL_MINOR_VERSION) + ")";
}

#endif
<|endoftext|>


<commit_before>/*
 * Copyright (C) 2013 by Glenn Hickey (hickey@soe.ucsc.edu)
 *
 * Released under the MIT license, see LICENSE.txt
 */

#include <cassert>
#include <deque>
#include "halLodExtract.h"
extern "C" {
#include "sonLibTree.h"
}

using namespace std;
using namespace hal;

LodExtract::LodExtract()
{

}

LodExtract::~LodExtract()
{

}

void LodExtract::createInterpolatedAlignment(AlignmentConstPtr inAlignment,
                                             AlignmentPtr outAlignment,
                                             hal_size_t step,
                                             const string& tree)
{
  _inAlignment = inAlignment;
  _outAlignment = outAlignment;

  string newTree = tree.empty() ? inAlignment->getNewickTree() : tree;
  createTree(newTree);
  cout << "tree = " << _outAlignment->getNewickTree() << endl;

  deque<string> bfQueue;
  bfQueue.push_front(_outAlignment->getRootName());
  while (!bfQueue.empty())
  {
    string genomeName = bfQueue.back();
    bfQueue.pop_back();
    vector<string> childNames = _outAlignment->getChildNames(genomeName);
    if (!childNames.empty())
    {
      convertInternalNode(genomeName, step);
      for (size_t childIdx = 0; childIdx < childNames.size(); childIdx++)
      {
        bfQueue.push_back(childNames[childIdx]);
      }
    }
  }
}

void LodExtract::createTree(const string& tree)
{
  if (_outAlignment->getNumGenomes() != 0)
  {
    throw hal_exception("Output alignment not empty");
  }
  stTree* root = stTree_parseNewickString(const_cast<char*>(tree.c_str()));
  assert(root != NULL);
  deque<stTree*> bfQueue;
  bfQueue.push_front(root);
  while (!bfQueue.empty())
  {
    stTree* node = bfQueue.back();
    const char* label = stTree_getLabel(node);
    if (label == NULL)
    {
      throw hal_exception("Error parsing tree: unlabeled node");
    }
    const Genome* test = _inAlignment->openGenome(label);
    if (test == NULL)
    {
      throw hal_exception(string("Genome in tree: ") + string(label) +
                          "doesn't exist in source alignment");
    }
    else
    {
      _inAlignment->closeGenome(test);
    }
    if (node == root)
    {
      _outAlignment->addRootGenome(label);
    }
    else
    {
      stTree* parent = stTree_getParent(node);
      assert(parent != NULL);
      const char* pLabel = stTree_getLabel(parent);
      assert(pLabel != NULL);
      double branchLength = stTree_getBranchLength(node);
      // clamp undefined branch lengths to 1. for now
      if (branchLength > 1e10)
      {
        branchLength = 1.;
      }
      _outAlignment->addLeafGenome(label, pLabel, branchLength);
    }

    int32_t numChildren = stTree_getChildNumber(node);
    for (int32_t childIdx = 0; childIdx < numChildren; ++childIdx)
    {
      bfQueue.push_front(stTree_getChild(node, childIdx));
    }

    bfQueue.pop_back();
  }
  stTree_destruct(root);
}

void LodExtract::convertInternalNode(const string& genomeName,
                                      hal_size_t step)
{
  const Genome* parent = _inAlignment->openGenome(genomeName);
  assert(parent != NULL);
  vector<string> childNames = _outAlignment->getChildNames(genomeName);
  vector<const Genome*> children;
  for (hal_size_t i = 0; i < childNames.size(); ++i)
  {
    children.push_back(_inAlignment->openGenome(childNames[i]));
  }
  _graph.build(_inAlignment, parent, children, step);

  map<const Sequence*, hal_size_t> segmentCounts;
  countSegmentsInGraph(segmentCounts);

  writeDimensions(segmentCounts, parent->getName(), childNames);
  writeSegments(parent, children);
  writeHomologies(parent, children);
  writeParseInfo(_outAlignment->openGenome(parent->getName()));

  // if we're gonna print anything out, do it before this:
  // (not necesssary but by closing genomes we erase their hdf5 caches
  // which can make a difference on huge trees
  _graph.erase();
  _outAlignment->closeGenome(_outAlignment->openGenome(parent->getName()));
  _inAlignment->closeGenome(parent);
  for (hal_size_t i = 0; i < children.size(); ++i)
  {
    _outAlignment->closeGenome(
      _outAlignment->openGenome(children[i]->getName()));
    _inAlignment->closeGenome(children[i]);
  }
}

void LodExtract::countSegmentsInGraph(
  map<const Sequence*, hal_size_t>& segmentCounts)
{
  const LodBlock* block;
  const LodSegment* segment;
  pair<map<const Sequence*, hal_size_t>::iterator, bool> res;

  for (hal_size_t blockIdx = 0; blockIdx < _graph.getNumBlocks(); ++blockIdx)
  {
    block = _graph.getBlock(blockIdx);
    for (hal_size_t segIdx = 0; segIdx < block->getNumSegments(); ++segIdx)
    {
      segment = block->getSegment(segIdx);
      res = segmentCounts.insert(pair<const Sequence*, hal_size_t>(
                                   segment->getSequence(), 0));
      hal_size_t& count = res.first->second;
      ++count;
    }
  }
}

void LodExtract::writeDimensions(
  const map<const Sequence*, hal_size_t>& segmentCounts,
  const string& parentName,
  const vector<string>& childNames)
{
  // initialize a dimensions list for each (input) genome
  map<const Genome*, vector<Sequence::Info> > dimMap;
  map<const Genome*, vector<Sequence::Info> >::iterator dimMapIt;
  vector<string> newGenomeNames = childNames;
  newGenomeNames.push_back(parentName);
  for (size_t i = 0; i < newGenomeNames.size(); ++i)
  {
    const Genome* inGenome = _inAlignment->openGenome(newGenomeNames[i]);
    pair<const Genome*, vector<Sequence::Info> > newEntry;
    newEntry.first = inGenome;
    dimMap.insert(newEntry);
  }

  // scan through the segment counts, adding the dimensions of each
  // sequence to the map entry for the appropriate genome.
  map<const Sequence*, hal_size_t>::const_iterator segMapIt;
  for (segMapIt = segmentCounts.begin(); segMapIt != segmentCounts.end();
       ++segMapIt)
  {
    const Sequence* inSequence = segMapIt->first;
    const Genome* inGenome = inSequence->getGenome();
    dimMapIt = dimMap.find(inGenome);
    assert(dimMapIt != dimMap.end());
    vector<Sequence::Info>& segDims = dimMapIt->second;
    hal_size_t nTop = inGenome->getName() == parentName ? 0 : segMapIt->second;
    hal_size_t nBot = inGenome->getName() != parentName ? 0 : segMapIt->second;
    segDims.push_back(Sequence::Info(inSequence->getName(),
                                     inSequence->getSequenceLength(),
                                     nTop,
                                     nBot));
  }

  // now that we have the dimensions for each genome, update them in
  // the output alignment
  for (dimMapIt = dimMap.begin(); dimMapIt != dimMap.end(); ++dimMapIt)
  {
    Genome* newGenome = _outAlignment->openGenome(dimMapIt->first->getName());
    assert(newGenome != NULL);
    vector<Sequence::Info>& segDims = dimMapIt->second;

    // ROOT
    if (newGenome->getName() == _outAlignment->getRootName())
    {
      assert(newGenome->getName() == parentName);
      newGenome->setDimensions(segDims);
    }
    // LEAF
    else if (newGenome->getName() != parentName)
    {
      newGenome->setDimensions(segDims);
    }
    // INTERNAL NODE
    else
    {
      vector<Sequence::UpdateInfo> updateInfo;
      for (size_t i = 0; i < segDims.size(); ++i)
      {
        updateInfo.push_back(
          Sequence::UpdateInfo(segDims[i]._name,
                               segDims[i]._numBottomSegments));
      }
      newGenome->updateBottomDimensions(updateInfo);
    }
  }
}

void LodExtract::writeSegments(const Genome* inParent,
                               const vector<const Genome*>& inChildren)
{
  vector<const Genome*> inGenomes = inChildren;
  inGenomes.push_back(inParent);
  const Genome* outParent = _outAlignment->openGenome(inParent->getName());
  assert(outParent != NULL && outParent->getNumBottomSegments() > 0);
  BottomSegmentIteratorPtr bottom;
  TopSegmentIteratorPtr top;
  SegmentIteratorPtr outSegment;

  // FOR EVERY GENOME
  for (hal_size_t i = 0; i < inGenomes.size(); ++i)
  {
    const Genome* inGenome = inGenomes[i];
    Genome* outGenome = _outAlignment->openGenome(inGenome->getName());

    if (outGenome != outParent)
    {
      top = outGenome->getTopSegmentIterator();
      outSegment = top;
    }
    else
    {
      bottom = outGenome->getBottomSegmentIterator();
      outSegment = bottom;
    }

    SequenceIteratorPtr outSeqIt = outGenome->getSequenceIterator();
    SequenceIteratorConstPtr outSeqEnd = outGenome->getSequenceEndIterator();

    // FOR EVERY SEQUENCE IN GENOME
    for (; outSeqIt != outSeqEnd; outSeqIt->toNext())
    {
      const Sequence* outSequence = outSeqIt->getSequence();
      const Sequence* inSequence =
         inGenome->getSequence(outSequence->getName());
      const LodGraph::SegmentSet* segSet = _graph.getSegmentSet(inSequence);
      assert(segSet != NULL);
      LodGraph::SegmentSet::const_iterator segIt = segSet->begin();
      //skip left telomere
      ++segIt;
      // use to skip right telomere:
      LodGraph::SegmentSet::const_iterator segLast = segSet->end();
      --segLast;

      // FOR EVERY SEGMENT IN SEQUENCE
      for (; segIt != segLast; ++segIt)
      {
        // write the HAL array index back to the segment to make
        // future passes quicker.
        (*segIt)->setArrayIndex(outSegment->getArrayIndex());
        outSegment->setCoordinates((*segIt)->getLeftPos(),
                                   (*segIt)->getLength());
        outSegment->toRight();
      }
    }
  }
}

void LodExtract::writeHomologies(const Genome* inParent,
                                 const vector<const Genome*>& inChildren)
{
  vector<const Genome*> inGenomes = inChildren;
  inGenomes.push_back(inParent);
  Genome* outParent = _outAlignment->openGenome(inParent->getName());
  assert(outParent != NULL && outParent->getNumBottomSegments() > 0);
  assert(inChildren.size() > 0);
  Genome* outChild = _outAlignment->openGenome(inChildren[0]->getName());
  BottomSegmentIteratorPtr bottom = outParent->getBottomSegmentIterator();
  TopSegmentIteratorPtr top = outChild->getTopSegmentIterator();

  // FOR EVERY BLOCK
  for (hal_size_t blockIdx = 0; blockIdx < _graph.getNumBlocks(); ++blockIdx)
  {
    SegmentMap segMap;
    const LodBlock* block = _graph.getBlock(blockIdx);

    for (hal_size_t segIdx = 0; segIdx < block->getNumSegments(); ++segIdx)
    {
      const LodSegment* segment = block->getSegment(segIdx);
      const Genome* genome = segment->getSequence()->getGenome();

      // ADD TO MAP
      pair<SegmentMap::iterator, bool> res = segMap.insert(
        pair<const Genome*, SegmentSet*>(genome, NULL));
      if (res.second == true)
      {
        assert(res.first->second == NULL);
        res.first->second = new SegmentSet();
      }
      res.first->second->insert(segment);
    }
    updateBlockEdges(inParent, segMap, block, bottom, top);
  }
}

void LodExtract::updateBlockEdges(const Genome* inParentGenome,
                                  SegmentMap& segMap,
                                  const LodBlock* block,
                                  BottomSegmentIteratorPtr bottom,
                                  TopSegmentIteratorPtr top)
{
  Genome* outParentGenome = bottom->getGenome();
  const LodSegment* rootSeg = NULL;
  SegmentSet* segSet;
  SegmentSet::iterator setIt;

  // Zap all segments in parent genome
  SegmentMap::iterator mapIt = segMap.find(inParentGenome);
  if (mapIt != segMap.end())
  {
    segSet = mapIt->second;
    assert(segSet != NULL);
    setIt = segSet->begin();
    for (; setIt != segSet->end(); ++setIt)
    {
      bottom->setArrayIndex(outParentGenome, (*setIt)->getArrayIndex());
      for (hal_size_t i = 0; i < bottom->getNumChildren(); ++i)
      {
        bottom->setChildIndex(i, NULL_INDEX);
        bottom->setTopParseIndex(NULL_INDEX);
      }
    }

    // Choose first segment as parent to all segments in the child genome
    setIt = segSet->begin();
    rootSeg = *(setIt);
    bottom->setArrayIndex(outParentGenome, (*setIt)->getArrayIndex());
  }

  // Do the child genomes
  SegmentSet::iterator nextIt;
  for (mapIt = segMap.begin(); mapIt != segMap.end(); ++mapIt)
  {
    if (mapIt->first != inParentGenome)
    {
      Genome* outChildGenome =
         _outAlignment->openGenome(mapIt->first->getName());
      hal_index_t childIndex = outParentGenome->getChildIndex(outChildGenome);
      assert(childIndex >= 0);
      segSet = mapIt->second;
      assert(segSet != NULL);
      for (setIt = segSet->begin(); setIt != segSet->end(); ++setIt)
      {
        top->setArrayIndex(outChildGenome, (*setIt)->getArrayIndex());
        top->setNextParalogyIndex(NULL_INDEX);

        // Connect to parent
        if (rootSeg != NULL)
        {
          top->setParentIndex(bottom->getArrayIndex());
          top->setBottomParseIndex(NULL_INDEX);
          bottom->setChildIndex(childIndex, top->getArrayIndex());
          bool reversed = (*setIt)->getFlipped() == rootSeg->getFlipped();
          top->setParentReversed(reversed);
          bottom->setChildReversed(childIndex, reversed);
        }
        else
        {
          top->setParentIndex(NULL_INDEX);
        }
      }
    }
  }
}

void LodExtract::writeParseInfo(Genome* genome)
{
  if (genome->getParent() == NULL || genome->getNumChildren() == 0)
  {
    return;
  }

 // copied from CactusHalConverter::updateRootParseInfo() in
  // cactus2hal/src/cactusHalConverter.cpp
  BottomSegmentIteratorPtr bottomIterator =
     genome->getBottomSegmentIterator();
  TopSegmentIteratorPtr topIterator = genome->getTopSegmentIterator();
  BottomSegmentIteratorConstPtr bend = genome->getBottomSegmentEndIterator();
  TopSegmentIteratorConstPtr tend = genome->getTopSegmentEndIterator();

  while (bottomIterator != bend && topIterator != tend)
  {
    bool bright = false;
    bool tright = false;
    BottomSegment* bseg = bottomIterator->getBottomSegment();
    TopSegment* tseg = topIterator->getTopSegment();
    hal_index_t bstart = bseg->getStartPosition();
    hal_index_t bend = bstart + (hal_index_t)bseg->getLength();
    hal_index_t tstart = tseg->getStartPosition();
    hal_index_t tend = tstart + (hal_index_t)tseg->getLength();

    if (bstart >= tstart && bstart < tend)
    {
      bseg->setTopParseIndex(tseg->getArrayIndex());
    }
    if (bend <= tend || bstart == bend)
    {
      bright = true;
    }

    if (tstart >= bstart && tstart < bend)
    {
      tseg->setBottomParseIndex(bseg->getArrayIndex());
    }
    if (tend <= bend || tstart == tend)
    {
      tright = true;
    }

    assert(bright || tright);
    if (bright == true)
    {
      bottomIterator->toRight();
    }
    if (tright == true)
    {
      topIterator->toRight();
    }
  }
}
<commit_msg>add paralogy links.<commit_after>/*
 * Copyright (C) 2013 by Glenn Hickey (hickey@soe.ucsc.edu)
 *
 * Released under the MIT license, see LICENSE.txt
 */

#include <cassert>
#include <deque>
#include "halLodExtract.h"
extern "C" {
#include "sonLibTree.h"
}

using namespace std;
using namespace hal;

LodExtract::LodExtract()
{

}

LodExtract::~LodExtract()
{

}

void LodExtract::createInterpolatedAlignment(AlignmentConstPtr inAlignment,
                                             AlignmentPtr outAlignment,
                                             hal_size_t step,
                                             const string& tree)
{
  _inAlignment = inAlignment;
  _outAlignment = outAlignment;

  string newTree = tree.empty() ? inAlignment->getNewickTree() : tree;
  createTree(newTree);
  cout << "tree = " << _outAlignment->getNewickTree() << endl;

  deque<string> bfQueue;
  bfQueue.push_front(_outAlignment->getRootName());
  while (!bfQueue.empty())
  {
    string genomeName = bfQueue.back();
    bfQueue.pop_back();
    vector<string> childNames = _outAlignment->getChildNames(genomeName);
    if (!childNames.empty())
    {
      convertInternalNode(genomeName, step);
      for (size_t childIdx = 0; childIdx < childNames.size(); childIdx++)
      {
        bfQueue.push_back(childNames[childIdx]);
      }
    }
  }
}

void LodExtract::createTree(const string& tree)
{
  if (_outAlignment->getNumGenomes() != 0)
  {
    throw hal_exception("Output alignment not empty");
  }
  stTree* root = stTree_parseNewickString(const_cast<char*>(tree.c_str()));
  assert(root != NULL);
  deque<stTree*> bfQueue;
  bfQueue.push_front(root);
  while (!bfQueue.empty())
  {
    stTree* node = bfQueue.back();
    const char* label = stTree_getLabel(node);
    if (label == NULL)
    {
      throw hal_exception("Error parsing tree: unlabeled node");
    }
    const Genome* test = _inAlignment->openGenome(label);
    if (test == NULL)
    {
      throw hal_exception(string("Genome in tree: ") + string(label) +
                          "doesn't exist in source alignment");
    }
    else
    {
      _inAlignment->closeGenome(test);
    }
    if (node == root)
    {
      _outAlignment->addRootGenome(label);
    }
    else
    {
      stTree* parent = stTree_getParent(node);
      assert(parent != NULL);
      const char* pLabel = stTree_getLabel(parent);
      assert(pLabel != NULL);
      double branchLength = stTree_getBranchLength(node);
      // clamp undefined branch lengths to 1. for now
      if (branchLength > 1e10)
      {
        branchLength = 1.;
      }
      _outAlignment->addLeafGenome(label, pLabel, branchLength);
    }

    int32_t numChildren = stTree_getChildNumber(node);
    for (int32_t childIdx = 0; childIdx < numChildren; ++childIdx)
    {
      bfQueue.push_front(stTree_getChild(node, childIdx));
    }

    bfQueue.pop_back();
  }
  stTree_destruct(root);
}

void LodExtract::convertInternalNode(const string& genomeName,
                                      hal_size_t step)
{
  const Genome* parent = _inAlignment->openGenome(genomeName);
  assert(parent != NULL);
  vector<string> childNames = _outAlignment->getChildNames(genomeName);
  vector<const Genome*> children;
  for (hal_size_t i = 0; i < childNames.size(); ++i)
  {
    children.push_back(_inAlignment->openGenome(childNames[i]));
  }
  _graph.build(_inAlignment, parent, children, step);

  map<const Sequence*, hal_size_t> segmentCounts;
  countSegmentsInGraph(segmentCounts);

  writeDimensions(segmentCounts, parent->getName(), childNames);
  writeSegments(parent, children);
  writeHomologies(parent, children);
  writeParseInfo(_outAlignment->openGenome(parent->getName()));

  // if we're gonna print anything out, do it before this:
  // (not necesssary but by closing genomes we erase their hdf5 caches
  // which can make a difference on huge trees
  _graph.erase();
  _outAlignment->closeGenome(_outAlignment->openGenome(parent->getName()));
  _inAlignment->closeGenome(parent);
  for (hal_size_t i = 0; i < children.size(); ++i)
  {
    _outAlignment->closeGenome(
      _outAlignment->openGenome(children[i]->getName()));
    _inAlignment->closeGenome(children[i]);
  }
}

void LodExtract::countSegmentsInGraph(
  map<const Sequence*, hal_size_t>& segmentCounts)
{
  const LodBlock* block;
  const LodSegment* segment;
  pair<map<const Sequence*, hal_size_t>::iterator, bool> res;

  for (hal_size_t blockIdx = 0; blockIdx < _graph.getNumBlocks(); ++blockIdx)
  {
    block = _graph.getBlock(blockIdx);
    for (hal_size_t segIdx = 0; segIdx < block->getNumSegments(); ++segIdx)
    {
      segment = block->getSegment(segIdx);
      res = segmentCounts.insert(pair<const Sequence*, hal_size_t>(
                                   segment->getSequence(), 0));
      hal_size_t& count = res.first->second;
      ++count;
    }
  }
}

void LodExtract::writeDimensions(
  const map<const Sequence*, hal_size_t>& segmentCounts,
  const string& parentName,
  const vector<string>& childNames)
{
  // initialize a dimensions list for each (input) genome
  map<const Genome*, vector<Sequence::Info> > dimMap;
  map<const Genome*, vector<Sequence::Info> >::iterator dimMapIt;
  vector<string> newGenomeNames = childNames;
  newGenomeNames.push_back(parentName);
  for (size_t i = 0; i < newGenomeNames.size(); ++i)
  {
    const Genome* inGenome = _inAlignment->openGenome(newGenomeNames[i]);
    pair<const Genome*, vector<Sequence::Info> > newEntry;
    newEntry.first = inGenome;
    dimMap.insert(newEntry);
  }

  // scan through the segment counts, adding the dimensions of each
  // sequence to the map entry for the appropriate genome.
  map<const Sequence*, hal_size_t>::const_iterator segMapIt;
  for (segMapIt = segmentCounts.begin(); segMapIt != segmentCounts.end();
       ++segMapIt)
  {
    const Sequence* inSequence = segMapIt->first;
    const Genome* inGenome = inSequence->getGenome();
    dimMapIt = dimMap.find(inGenome);
    assert(dimMapIt != dimMap.end());
    vector<Sequence::Info>& segDims = dimMapIt->second;
    hal_size_t nTop = inGenome->getName() == parentName ? 0 : segMapIt->second;
    hal_size_t nBot = inGenome->getName() != parentName ? 0 : segMapIt->second;
    segDims.push_back(Sequence::Info(inSequence->getName(),
                                     inSequence->getSequenceLength(),
                                     nTop,
                                     nBot));
  }

  // now that we have the dimensions for each genome, update them in
  // the output alignment
  for (dimMapIt = dimMap.begin(); dimMapIt != dimMap.end(); ++dimMapIt)
  {
    Genome* newGenome = _outAlignment->openGenome(dimMapIt->first->getName());
    assert(newGenome != NULL);
    vector<Sequence::Info>& segDims = dimMapIt->second;

    // ROOT
    if (newGenome->getName() == _outAlignment->getRootName())
    {
      assert(newGenome->getName() == parentName);
      newGenome->setDimensions(segDims);
    }
    // LEAF
    else if (newGenome->getName() != parentName)
    {
      newGenome->setDimensions(segDims);
    }
    // INTERNAL NODE
    else
    {
      vector<Sequence::UpdateInfo> updateInfo;
      for (size_t i = 0; i < segDims.size(); ++i)
      {
        updateInfo.push_back(
          Sequence::UpdateInfo(segDims[i]._name,
                               segDims[i]._numBottomSegments));
      }
      newGenome->updateBottomDimensions(updateInfo);
    }
  }
}

void LodExtract::writeSegments(const Genome* inParent,
                               const vector<const Genome*>& inChildren)
{
  vector<const Genome*> inGenomes = inChildren;
  inGenomes.push_back(inParent);
  const Genome* outParent = _outAlignment->openGenome(inParent->getName());
  assert(outParent != NULL && outParent->getNumBottomSegments() > 0);
  BottomSegmentIteratorPtr bottom;
  TopSegmentIteratorPtr top;
  SegmentIteratorPtr outSegment;

  // FOR EVERY GENOME
  for (hal_size_t i = 0; i < inGenomes.size(); ++i)
  {
    const Genome* inGenome = inGenomes[i];
    Genome* outGenome = _outAlignment->openGenome(inGenome->getName());

    if (outGenome != outParent)
    {
      top = outGenome->getTopSegmentIterator();
      outSegment = top;
    }
    else
    {
      bottom = outGenome->getBottomSegmentIterator();
      outSegment = bottom;
    }

    SequenceIteratorPtr outSeqIt = outGenome->getSequenceIterator();
    SequenceIteratorConstPtr outSeqEnd = outGenome->getSequenceEndIterator();

    // FOR EVERY SEQUENCE IN GENOME
    for (; outSeqIt != outSeqEnd; outSeqIt->toNext())
    {
      const Sequence* outSequence = outSeqIt->getSequence();
      const Sequence* inSequence =
         inGenome->getSequence(outSequence->getName());
      const LodGraph::SegmentSet* segSet = _graph.getSegmentSet(inSequence);
      assert(segSet != NULL);
      LodGraph::SegmentSet::const_iterator segIt = segSet->begin();
      //skip left telomere
      ++segIt;
      // use to skip right telomere:
      LodGraph::SegmentSet::const_iterator segLast = segSet->end();
      --segLast;

      // FOR EVERY SEGMENT IN SEQUENCE
      for (; segIt != segLast; ++segIt)
      {
        // write the HAL array index back to the segment to make
        // future passes quicker.
        (*segIt)->setArrayIndex(outSegment->getArrayIndex());
        outSegment->setCoordinates((*segIt)->getLeftPos(),
                                   (*segIt)->getLength());
        outSegment->toRight();
      }
    }
  }
}

void LodExtract::writeHomologies(const Genome* inParent,
                                 const vector<const Genome*>& inChildren)
{
  vector<const Genome*> inGenomes = inChildren;
  inGenomes.push_back(inParent);
  Genome* outParent = _outAlignment->openGenome(inParent->getName());
  assert(outParent != NULL && outParent->getNumBottomSegments() > 0);
  assert(inChildren.size() > 0);
  Genome* outChild = _outAlignment->openGenome(inChildren[0]->getName());
  BottomSegmentIteratorPtr bottom = outParent->getBottomSegmentIterator();
  TopSegmentIteratorPtr top = outChild->getTopSegmentIterator();

  // FOR EVERY BLOCK
  for (hal_size_t blockIdx = 0; blockIdx < _graph.getNumBlocks(); ++blockIdx)
  {
    SegmentMap segMap;
    const LodBlock* block = _graph.getBlock(blockIdx);

    for (hal_size_t segIdx = 0; segIdx < block->getNumSegments(); ++segIdx)
    {
      const LodSegment* segment = block->getSegment(segIdx);
      const Genome* genome = segment->getSequence()->getGenome();

      // ADD TO MAP
      pair<SegmentMap::iterator, bool> res = segMap.insert(
        pair<const Genome*, SegmentSet*>(genome, NULL));
      if (res.second == true)
      {
        assert(res.first->second == NULL);
        res.first->second = new SegmentSet();
      }
      res.first->second->insert(segment);
    }
    updateBlockEdges(inParent, segMap, block, bottom, top);
  }
}

void LodExtract::updateBlockEdges(const Genome* inParentGenome,
                                  SegmentMap& segMap,
                                  const LodBlock* block,
                                  BottomSegmentIteratorPtr bottom,
                                  TopSegmentIteratorPtr top)
{
  Genome* outParentGenome = bottom->getGenome();
  const LodSegment* rootSeg = NULL;
  SegmentSet* segSet;
  SegmentSet::iterator setIt;

  // Zap all segments in parent genome
  SegmentMap::iterator mapIt = segMap.find(inParentGenome);
  if (mapIt != segMap.end())
  {
    segSet = mapIt->second;
    assert(segSet != NULL);
    setIt = segSet->begin();
    for (; setIt != segSet->end(); ++setIt)
    {
      bottom->setArrayIndex(outParentGenome, (*setIt)->getArrayIndex());
      for (hal_size_t i = 0; i < bottom->getNumChildren(); ++i)
      {
        bottom->setChildIndex(i, NULL_INDEX);
        bottom->setTopParseIndex(NULL_INDEX);
      }
    }

    // Choose first segment as parent to all segments in the child genome
    setIt = segSet->begin();
    rootSeg = *(setIt);
    bottom->setArrayIndex(outParentGenome, (*setIt)->getArrayIndex());
  }

  // Do the child genomes
  SegmentSet::iterator nextIt;
  for (mapIt = segMap.begin(); mapIt != segMap.end(); ++mapIt)
  {
    if (mapIt->first != inParentGenome)
    {
      Genome* outChildGenome =
         _outAlignment->openGenome(mapIt->first->getName());
      hal_index_t childIndex = outParentGenome->getChildIndex(outChildGenome);
      assert(childIndex >= 0);
      segSet = mapIt->second;
      assert(segSet != NULL);
      for (setIt = segSet->begin(); setIt != segSet->end(); ++setIt)
      {
        top->setArrayIndex(outChildGenome, (*setIt)->getArrayIndex());
        top->setBottomParseIndex(NULL_INDEX);

        // Connect to parent
        if (rootSeg != NULL)
        {
          top->setParentIndex(bottom->getArrayIndex());
          bool reversed = (*setIt)->getFlipped() == rootSeg->getFlipped();
          top->setParentReversed(reversed);
          if (setIt == segSet->begin())
          {
            bottom->setChildIndex(childIndex, top->getArrayIndex());
            bottom->setChildReversed(childIndex, reversed);
          }
        }
        else
        {
          top->setParentIndex(NULL_INDEX);
        }

        // Connect to next paralogy
        SegmentSet::iterator setNext = setIt;
        ++setNext;
        if (setNext == segSet->end())
        {
          setNext = segSet->begin();
        }
        if (setNext == setIt)
        {
          top->setNextParalogyIndex(NULL_INDEX);
        }
        else
        {
          top->setNextParalogyIndex((*setNext)->getArrayIndex());
        }
      }
    }
  }
}

void LodExtract::writeParseInfo(Genome* genome)
{
  if (genome->getParent() == NULL || genome->getNumChildren() == 0)
  {
    return;
  }

 // copied from CactusHalConverter::updateRootParseInfo() in
  // cactus2hal/src/cactusHalConverter.cpp
  BottomSegmentIteratorPtr bottomIterator =
     genome->getBottomSegmentIterator();
  TopSegmentIteratorPtr topIterator = genome->getTopSegmentIterator();
  BottomSegmentIteratorConstPtr bend = genome->getBottomSegmentEndIterator();
  TopSegmentIteratorConstPtr tend = genome->getTopSegmentEndIterator();

  while (bottomIterator != bend && topIterator != tend)
  {
    bool bright = false;
    bool tright = false;
    BottomSegment* bseg = bottomIterator->getBottomSegment();
    TopSegment* tseg = topIterator->getTopSegment();
    hal_index_t bstart = bseg->getStartPosition();
    hal_index_t bend = bstart + (hal_index_t)bseg->getLength();
    hal_index_t tstart = tseg->getStartPosition();
    hal_index_t tend = tstart + (hal_index_t)tseg->getLength();

    if (bstart >= tstart && bstart < tend)
    {
      bseg->setTopParseIndex(tseg->getArrayIndex());
    }
    if (bend <= tend || bstart == bend)
    {
      bright = true;
    }

    if (tstart >= bstart && tstart < bend)
    {
      tseg->setBottomParseIndex(bseg->getArrayIndex());
    }
    if (tend <= bend || tstart == tend)
    {
      tright = true;
    }

    assert(bright || tright);
    if (bright == true)
    {
      bottomIterator->toRight();
    }
    if (tright == true)
    {
      topIterator->toRight();
    }
  }
}
<|endoftext|>


<commit_before>/* Wave shaping class, by John Gibson, 1999. Derived from cmix genlib/wshape.c.
*/

#include "WavShape.h"

WavShape :: WavShape(MY_FLOAT aLimit = 1.0)
{
   limit = aLimit;
   transferFunc = NULL;
}

WavShape :: ~WavShape()
{
}

/* <aFunc> is an array of <aSize> floats in the range [-1, 1].
   Caller must make sure this storage remains valid for the life of
   the object.
*/
void WavShape :: setTransferFunc(MY_FLOAT *aFunc, int aSize)
{
   transferFunc = aFunc;
   lastIndex = aSize - 1;
   indexFactor = lastIndex / 2;
}

MY_FLOAT WavShape :: tick(MY_FLOAT sample)
{
   int      loc1;
   MY_FLOAT findex, frac, val1, val2;

   if (transferFunc) {
      findex = (sample + limit) * indexFactor;
      if (findex < 0.0)
         findex = 0.0;

      loc1 = (int)findex;
      frac = findex - (MY_FLOAT)loc1;
      val1 = transferFunc[loc1];
      if (loc1 < lastIndex)
         val2 = transferFunc[loc1 + 1];
      else
         val2 = val1;
      lastOutput = val1 + frac * (val2 - val1);
   }
   else
      lastOutput = sample;

   return lastOutput;
}

<commit_msg>Protect against out-of-range samples.<commit_after>/* Wave shaping class, by John Gibson, 1999. Derived from cmix genlib/wshape.c.
*/

#include "WavShape.h"

WavShape :: WavShape(MY_FLOAT aLimit = 1.0)
{
   limit = aLimit;
   transferFunc = NULL;
}

WavShape :: ~WavShape()
{
}

/* <aFunc> is an array of <aSize> floats in the range [-1, 1].
   Caller must make sure this storage remains valid for the life of
   the object.
*/
void WavShape :: setTransferFunc(MY_FLOAT *aFunc, int aSize)
{
   transferFunc = aFunc;
   lastIndex = aSize - 1;
   indexFactor = lastIndex / 2;
}

MY_FLOAT WavShape :: tick(MY_FLOAT sample)
{
   int      loc1;
   MY_FLOAT findex, frac, val1, val2;

   if (transferFunc) {
      findex = (sample + limit) * indexFactor;
      if (findex < 0.0)
         findex = 0.0;

      loc1 = (int)findex;
      frac = findex - (MY_FLOAT)loc1;
      if (loc1 < lastIndex) {
         val1 = transferFunc[loc1];
         val2 = transferFunc[loc1 + 1];
      }
      else     // pin to last array value if sample out of range
         val2 = val1 = transferFunc[lastIndex];
      lastOutput = val1 + frac * (val2 - val1);
   }
   else
      lastOutput = sample;

   return lastOutput;
}

<|endoftext|>


<commit_before>#include "peripheralfactory.h"
#include "micromaestro12.h"

#define MM16PORT "/dev/ttyACM1"
using namespace NRMCHardware;

PeripheralFactory& PeripheralFactory::getInstance (  )
{
	static PeripheralFactory instance;

	return instance;
}

SmrtPeripheral* PeripheralFactory::getPeripheral ( PeripheralSystem sys, bool forceConnect)
{
	SmrtPeripheral* peripheral = forceConnect ? 0 : checkPool(sys);

	//check if there was a peroperal of the requested type in the pool and if not attempt to connect a periperal of that type
	if (peripheral == 0)
		peripheral = connectPeripheral(sys);

	return peripheral;
}

void PeripheralFactory::attachFactory ( SerialPortFactory& factory )
{
	// check if a fectory is attached
	if (srlPrtFactory != 0)
	{
		// dispose of the attached factory
		delete srlPrtFactory;
		srlPrtFactory = 0;
	}

	srlPrtFactory = &factory;
}

void PeripheralFactory::returnPeripheral ( Peripheral& peripheral )
{
	peripheralPool.push_back(&peripheral);
}

PeripheralFactory::PeripheralFactory (  )
{
	srlPrtFactory = 0;
}

PeripheralFactory::~PeripheralFactory (  )
{
	srlPrtFactory = 0;

	// destroy the pool
	for (unsigned int index = 0; index < peripheralPool.size(); index++)
		delete peripheralPool[index];

	peripheralPool.clear();
}

SmrtPeripheral * NRMCHardware::PeripheralFactory::checkPool(PeripheralSystem sys)
{
	SmrtPeripheral* periperal = 0;

	if (!peripheralPool.empty())
	{
		// search if there is already a peripheral in the pool
		for (vector<Peripheral*>::iterator it = peripheralPool.begin(); it != peripheralPool.end(); ++it)
		{
			if ((*it)->getSystem() == sys)
			{
				periperal = new SmrtPeripheral(*(*it), *this);
				peripheralPool.erase(it);
				break;
			}
		}
	}

	return periperal;
}

SmrtPeripheral * NRMCHardware::PeripheralFactory::connectPeripheral(PeripheralSystem sys)
{
	Peripheral* tmp = 0;
	SmrtPeripheral* periperal = 0;

	switch (sys)
	{
	case NRMCHardware::LocomotionSystem:
		tmp = new MicroMaestro12(srlPrtFactory->openPort(MM16PORT, 9600), NRMCHardware::LocomotionSystem);	// we use a micromaestro for servo and motor control
		break;
	case NRMCHardware::MSP430SensorSystem:
		break;
	case NRMCHardware::ObsticalDetectionLIDAR:
		break;
	case NRMCHardware::LocalizationLIDAR:
		break;
	}

	if (tmp != 0)
		periperal = new SmrtPeripheral(*tmp, *this);

	return periperal;
}

<commit_msg>Changed default serial port from ttyACM1 to ttyACM0<commit_after>#include "peripheralfactory.h"
#include "micromaestro12.h"

#define MM16PORT "/dev/ttyACM0"
using namespace NRMCHardware;

PeripheralFactory& PeripheralFactory::getInstance (  )
{
	static PeripheralFactory instance;

	return instance;
}

SmrtPeripheral* PeripheralFactory::getPeripheral ( PeripheralSystem sys, bool forceConnect)
{
	SmrtPeripheral* peripheral = forceConnect ? 0 : checkPool(sys);

	//check if there was a peroperal of the requested type in the pool and if not attempt to connect a periperal of that type
	if (peripheral == 0)
		peripheral = connectPeripheral(sys);

	return peripheral;
}

void PeripheralFactory::attachFactory ( SerialPortFactory& factory )
{
	// check if a fectory is attached
	if (srlPrtFactory != 0)
	{
		// dispose of the attached factory
		delete srlPrtFactory;
		srlPrtFactory = 0;
	}

	srlPrtFactory = &factory;
}

void PeripheralFactory::returnPeripheral ( Peripheral& peripheral )
{
	peripheralPool.push_back(&peripheral);
}

PeripheralFactory::PeripheralFactory (  )
{
	srlPrtFactory = 0;
}

PeripheralFactory::~PeripheralFactory (  )
{
	srlPrtFactory = 0;

	// destroy the pool
	for (unsigned int index = 0; index < peripheralPool.size(); index++)
		delete peripheralPool[index];

	peripheralPool.clear();
}

SmrtPeripheral * NRMCHardware::PeripheralFactory::checkPool(PeripheralSystem sys)
{
	SmrtPeripheral* periperal = 0;

	if (!peripheralPool.empty())
	{
		// search if there is already a peripheral in the pool
		for (vector<Peripheral*>::iterator it = peripheralPool.begin(); it != peripheralPool.end(); ++it)
		{
			if ((*it)->getSystem() == sys)
			{
				periperal = new SmrtPeripheral(*(*it), *this);
				peripheralPool.erase(it);
				break;
			}
		}
	}

	return periperal;
}

SmrtPeripheral * NRMCHardware::PeripheralFactory::connectPeripheral(PeripheralSystem sys)
{
	Peripheral* tmp = 0;
	SmrtPeripheral* periperal = 0;

	switch (sys)
	{
	case NRMCHardware::LocomotionSystem:
		tmp = new MicroMaestro12(srlPrtFactory->openPort(MM16PORT, 9600), NRMCHardware::LocomotionSystem);	// we use a micromaestro for servo and motor control
		break;
	case NRMCHardware::MSP430SensorSystem:
		break;
	case NRMCHardware::ObsticalDetectionLIDAR:
		break;
	case NRMCHardware::LocalizationLIDAR:
		break;
	}

	if (tmp != 0)
		periperal = new SmrtPeripheral(*tmp, *this);

	return periperal;
}

<|endoftext|>