<commit_before>/*
  Automaton.cpp - Library for creating and running Finite State Machines.
  Published under the MIT License (MIT), Copyright (c) 2015, J.P. van der Landen
*/

#include "Automaton.h"

void atm_timer_millis::set( uint32_t v ) {
    value = v;
}

void atm_timer_micros::set( uint32_t v ) {
    value = v;
}

int atm_timer_millis::expired( BaseMachine * machine ) {
    return value == ATM_TIMER_OFF ? 0 : millis() - machine->state_millis >= value;
}

int atm_timer_micros::expired( BaseMachine * machine ) {
    return value == ATM_TIMER_OFF ? 0 : micros() - machine->state_micros >= value;
}

void atm_counter::set( uint16_t v )
{
    value = v;
}

uint16_t atm_counter::decrement( void )
{
    return value > 0 && value != ATM_COUNTER_OFF ? --value : 0;
}

uint8_t atm_counter::expired()
{
    return value == ATM_COUNTER_OFF ? 0 : ( value > 0 ? 0 : 1 );
}

Machine & Machine::state(state_t state)
{
    next = state;
    last_trigger = -1;
    flags &= ~ATM_SLEEP_FLAG;
    if ( ( flags & ATM_MSGAC_FLAG ) > 0 ) msgClear();
    return *this;
}

state_t Machine::state()
{
    return current;
}

int Machine::trigger( int evt )
{
    if ( current > -1 ) {
        int new_state = read_state( state_table + ( current * state_width ) + evt + ATM_ON_EXIT + 1 );
        if ( new_state > -1 ) {
	      state( new_state );
          last_trigger = evt;
          return 1;
        }
    }
    return 0;
}

Machine &  Machine::toggle( state_t state1, state_t state2 )
{
    state( current == state1 ? state2 : state1 );
    return *this;
}

Machine & Machine::onSwitch( swcb_sym_t callback, const char sym_s[], const char sym_e[] )
{
    callback_sym = callback;
    sym_states = sym_s;
    sym_events = sym_e;
    return *this;
}

Machine & Machine::label( const char label[] )
{
    inst_label = label;
    return *this;
}

int8_t Machine::priority()
{
    return prio;
}

Machine & Machine::priority( int8_t priority )
{
    prio = priority;
    return *this;
}

// .asleep() Returns true if the machine is in a sleeping state

uint8_t BaseMachine::asleep()
{
    return ( flags & ATM_SLEEP_FLAG ) > 0;
}

BaseMachine & BaseMachine::sleep()
{
    flags |=  ATM_SLEEP_FLAG;
    return *this;
}

Machine & Machine::begin( const state_t* tbl, int width )
{
    state_table = tbl;
    state_width = ATM_ON_EXIT + width + 2;
    prio = ATM_DEFAULT_PRIO;
    if ( !inst_label ) inst_label = class_label;
    return *this;
}

Machine & Machine::msgQueue( atm_msg_t msg[], int width )
{
    msg_table = msg;
    msg_width = width;
    return *this;
}

Machine & Machine::msgQueue( atm_msg_t msg[], int width, uint8_t autoclear )
{
    msg_table = msg;
    msg_width = width;
    flags = autoclear ? flags | ATM_MSGAC_FLAG : flags & ~ATM_MSGAC_FLAG;
    return *this;
}

unsigned char Machine::pinChange( uint8_t pin ) {

  unsigned char v = digitalRead( pin ) ? 1 : 0;
  if ( (( pinstate >> pin ) & 1 ) != ( v == 1 ) ) {
    pinstate ^= ( (uint32_t)1 << pin );
    return 1;
  }
  return 0;
}

int Machine::msgRead( uint8_t id_msg ) // Checks msg queue and removes 1 msg
{
  if ( msg_table[id_msg] > 0 ) {
      msg_table[id_msg]--;
      return 1;
  }
  return 0;
}

int Machine::msgRead( uint8_t id_msg, int cnt )
{
  if ( msg_table[id_msg] > 0 ) {
    if ( cnt >= msg_table[id_msg] ) {
        msg_table[id_msg] = 0;
    } else {
        msg_table[id_msg] -= cnt;
    }
    return 1;
  }
  return 0;
}

int Machine::msgRead( uint8_t id_msg, int cnt, int clear )
{
  if ( msg_table[id_msg] > 0 ) {
    if ( cnt >= msg_table[id_msg] ) {
        msg_table[id_msg] = 0;
    } else {
        msg_table[id_msg] -= cnt;
    }
	if ( clear ) {
		for ( int i = 0; i < msg_width; i++ ) {
			msg_table[i] = 0;
		}
    }
    return 1;
  }
  return 0;
}

int Machine::msgPeek( uint8_t id_msg )
{
  return msg_table[id_msg];
}

int Machine::msgClear( uint8_t id_msg ) // Destructive read (clears queue for this type)
{
  flags &= ~ATM_SLEEP_FLAG;
  if ( msg_table[id_msg] > 0 ) {
    msg_table[id_msg] = 0;
    return 1;
  }
  return 0;
}

Machine & Machine::msgClear()
{
  flags &= ~ATM_SLEEP_FLAG;
  for ( int i = 0; i < msg_width; i++ ) {
    msg_table[i] = 0;
  }
  return *this;
}

Machine & Machine::msgWrite( uint8_t id_msg )
{
  flags &= ~ATM_SLEEP_FLAG;
  msg_table[id_msg]++;
  return *this;
}

Machine & Machine::msgWrite( uint8_t id_msg, int cnt )
{
  flags &= ~ATM_SLEEP_FLAG;
  msg_table[id_msg] += cnt;
  return *this;
}

const char * Machine::map_symbol( int id, const char map[] )
{
  int cnt = 0;
  int i = 0;
  if ( id == -1 ) return "*NONE*";
  if ( id ==  0 ) return map;
  while ( 1 ) {
    if ( map[i] == '\0' && ++cnt == id ) {
      i++;
      break;
    }
    i++;
  }
  return &map[i];
}

// .cycle() Executes one cycle of a state machine
Machine & Machine::cycle()
{
    if ( ( flags & ATM_SLEEP_FLAG ) == 0 ) {
        cycles++;
        if ( next != -1 ) {
            action( ATM_ON_SWITCH );
            if ( callback_sym ) {
                callback_sym( inst_label,
                    map_symbol(      current, sym_states ),
                    map_symbol(         next, sym_states ),
                    map_symbol( last_trigger, sym_events ), millis() - state_millis, cycles );
            }
            if ( current > -1 )
		action( read_state( state_table + ( current * state_width ) + ATM_ON_EXIT ) );
            previous = current;
            current = next;
            next = -1;
            state_millis = millis();
            state_micros = micros();
            action( read_state( state_table + ( current * state_width ) + ATM_ON_ENTER ) );
            if ( read_state( state_table + ( current * state_width ) + ATM_ON_LOOP ) == ATM_SLEEP ) {
                  flags |= ATM_SLEEP_FLAG;
            } else {
                  flags &= ~ATM_SLEEP_FLAG;
            }
            cycles = 0;
        }
        state_t i = read_state( state_table + ( current * state_width ) + ATM_ON_LOOP );
        if ( i != -1 ) { action( i ); }
        for ( i = ATM_ON_EXIT + 1; i < state_width; i++ ) {
            if ( ( read_state( state_table + ( current * state_width ) + i ) != -1 ) && ( i == state_width - 1 || event( i - ATM_ON_EXIT - 1 ) ) ) {
                state( read_state( state_table + ( current * state_width ) + i ) );
                last_trigger = i - ATM_ON_EXIT - 1;
                return *this;
            }
        }
    }
    return *this;
}

// TINY MACHINE

TinyMachine & TinyMachine::state(tiny_state_t state)
{
    next = state;
    flags &= ~ATM_SLEEP_FLAG;
    return *this;
}

tiny_state_t TinyMachine::state()
{
    return current;
}

int TinyMachine::trigger( int evt )
{
    if ( current > -1 ) {
        int new_state = tiny_read_state( state_table + ( current * state_width ) + evt + ATM_ON_EXIT + 1 );
        if ( new_state > -1 ) {
          state( new_state );
          return 1;
        }
    }
    return 0;
}

TinyMachine & TinyMachine::begin( const tiny_state_t* tbl, int width )
{
    state_table = tbl;
    state_width = ATM_ON_EXIT + width + 2;
    return *this;
}

// .cycle() Executes one cycle of a state machine
TinyMachine & TinyMachine::cycle()
{
    if ( ( flags & ATM_SLEEP_FLAG ) == 0 ) {
        if ( next != -1 ) {
            action( ATM_ON_SWITCH );
            if ( current > -1 )
                action( tiny_read_state( state_table + ( current * state_width ) + ATM_ON_EXIT ) );
            current = next;
            next = -1;
            state_millis = millis();
            state_micros = micros();
            action( tiny_read_state( state_table + ( current * state_width ) + ATM_ON_ENTER ) );
            if ( read_state( state_table + ( current * state_width ) + ATM_ON_LOOP ) == ATM_SLEEP ) {
                  flags |= ATM_SLEEP_FLAG;
            } else {
                  flags &= ~ATM_SLEEP_FLAG;
            }
        }
        tiny_state_t i = tiny_read_state( state_table + ( current * state_width ) + ATM_ON_LOOP );
        if ( i != -1 ) { action( i ); }
        for ( i = ATM_ON_EXIT + 1; i < state_width; i++ ) {
            if ( ( tiny_read_state( state_table + ( current * state_width ) + i ) != -1 ) && ( i == state_width - 1 || event( i - ATM_ON_EXIT - 1 ) ) ) {
                state( tiny_read_state( state_table + ( current * state_width ) + i ) );
                return *this;
            }
        }
    }
    return *this;
}

// FACTORY

// .calibrate() Distributes the machines in the inventory to the appropriate priority queues
void Factory::calibrate( void )
{
    // Reset all priority queues to empty lists
    for ( int8_t i = 0; i < ATM_NO_OF_QUEUES; i++ )
        priority_root[i] = 0;
    // Walk the inventory list that contains all state machines in this factory
    Machine * m = inventory_root;
    while ( m ) {
        // Prepend every machine to the appropriate priority queue
        if ( m->prio < ATM_NO_OF_QUEUES ) {
            m->priority_next = priority_root[m->prio];
            priority_root[m->prio] = m;
        }
        m = m->inventory_next;
    }
    recalibrate = 0;
}

// .run( q ) Traverses an individual priority queue and cycles the machines in it once (except for queue 0)
void Factory::run( int q )
{
    Machine * m = priority_root[ q ];
    while ( m ) {
        if ( q > 0 && ( m->flags & ATM_SLEEP_FLAG ) == 0 ) m->cycle();
        // Request a recalibrate if the prio field doesn't match the current queue
        if ( m->prio != q ) recalibrate = 1;
        // Move to the next machine
        m = m->priority_next;
    }
}

// .add( machine ) Adds a state machine to the factory by prepending it to the inventory list
Factory & Factory::add( Machine & machine )
{
    machine.inventory_next = inventory_root;
    inventory_root = &machine;
    machine.factory = this;
    recalibrate = 1;
    machine.cycle();
    return *this;
}

// .find() Search the factory inventory for a machine by instance label
Machine * Factory::find( const char label[] )
{
    Machine * m = inventory_root;
    while ( m ) {
        if ( strcmp( label, m->inst_label ) == 0 ) {
            return m;
        }
        m = m->inventory_next;
    }
    return 0;
}

int Factory::msgSend( const char label[], int msg, int cnt )
{
    int r = 0;
    Machine * m = inventory_root;
    while ( m ) {
        if ( strcmp( label, m->inst_label ) == 0 ) {
            m->msgWrite( msg, cnt );
            r++;
        }
        m = m->inventory_next;
    }
    return r;
}

int Factory::msgSend( const char label[], int msg )
{
    msgSend( label, msg, 1 );
}

int Factory::msgSendClass( const char label[], int msg, int cnt )
{
    int r = 0;
    Machine * m = inventory_root;
    while ( m ) {
        if ( strcmp( label, m->class_label ) == 0 ) {
            m->msgWrite( msg, cnt );
            r++;
        }
        m = m->inventory_next;
    }
    return r;
}

int Factory::msgSendClass( const char label[], int msg )
{
    msgSendClass( label, msg, 1 );
}

// .cycle() executes one factory cycle (runs all priority queues a certain number of times)
Factory & Factory::cycle( void )
{
    if ( recalibrate ) calibrate();
    run( 1 ); run( 2 );	run( 1 ); run( 2 );
    run( 1 ); run( 3 );	run( 1 ); run( 4 );
    run( 1 ); run( 2 );	run( 1 ); run( 3 );
    run( 1 ); run( 2 );	run( 1 ); run( 0 );
    return  *this;
}

<commit_msg>Automaton: cycle() on factory.add() removed, trigger now cycles on invalid state<commit_after>/*
  Automaton.cpp - Library for creating and running Finite State Machines.
  Published under the MIT License (MIT), Copyright (c) 2015, J.P. van der Landen
*/

#include "Automaton.h"

void atm_timer_millis::set( uint32_t v ) {
    value = v;
}

void atm_timer_micros::set( uint32_t v ) {
    value = v;
}

int atm_timer_millis::expired( BaseMachine * machine ) {
    return value == ATM_TIMER_OFF ? 0 : millis() - machine->state_millis >= value;
}

int atm_timer_micros::expired( BaseMachine * machine ) {
    return value == ATM_TIMER_OFF ? 0 : micros() - machine->state_micros >= value;
}

void atm_counter::set( uint16_t v )
{
    value = v;
}

uint16_t atm_counter::decrement( void )
{
    return value > 0 && value != ATM_COUNTER_OFF ? --value : 0;
}

uint8_t atm_counter::expired()
{
    return value == ATM_COUNTER_OFF ? 0 : ( value > 0 ? 0 : 1 );
}

Machine & Machine::state(state_t state)
{
    next = state;
    last_trigger = -1;
    flags &= ~ATM_SLEEP_FLAG;
    if ( ( flags & ATM_MSGAC_FLAG ) > 0 ) msgClear();
    return *this;
}

state_t Machine::state()
{
    return current;
}

int Machine::trigger( int evt )
{
    if ( current == -1 ) cycle();
    if ( current > -1 ) {
        int new_state = read_state( state_table + ( current * state_width ) + evt + ATM_ON_EXIT + 1 );
        if ( new_state > -1 ) {
	      state( new_state );
          last_trigger = evt;
          return 1;
        }
    }
    return 0;
}

Machine &  Machine::toggle( state_t state1, state_t state2 )
{
    state( current == state1 ? state2 : state1 );
    return *this;
}

Machine & Machine::onSwitch( swcb_sym_t callback, const char sym_s[], const char sym_e[] )
{
    callback_sym = callback;
    sym_states = sym_s;
    sym_events = sym_e;
    return *this;
}

Machine & Machine::label( const char label[] )
{
    inst_label = label;
    return *this;
}

int8_t Machine::priority()
{
    return prio;
}

Machine & Machine::priority( int8_t priority )
{
    prio = priority;
    return *this;
}

// .asleep() Returns true if the machine is in a sleeping state

uint8_t BaseMachine::asleep()
{
    return ( flags & ATM_SLEEP_FLAG ) > 0;
}

BaseMachine & BaseMachine::sleep()
{
    flags |=  ATM_SLEEP_FLAG;
    return *this;
}

Machine & Machine::begin( const state_t* tbl, int width )
{
    state_table = tbl;
    state_width = ATM_ON_EXIT + width + 2;
    prio = ATM_DEFAULT_PRIO;
    if ( !inst_label ) inst_label = class_label;
    return *this;
}

Machine & Machine::msgQueue( atm_msg_t msg[], int width )
{
    msg_table = msg;
    msg_width = width;
    return *this;
}

Machine & Machine::msgQueue( atm_msg_t msg[], int width, uint8_t autoclear )
{
    msg_table = msg;
    msg_width = width;
    flags = autoclear ? flags | ATM_MSGAC_FLAG : flags & ~ATM_MSGAC_FLAG;
    return *this;
}

unsigned char Machine::pinChange( uint8_t pin ) {

  unsigned char v = digitalRead( pin ) ? 1 : 0;
  if ( (( pinstate >> pin ) & 1 ) != ( v == 1 ) ) {
    pinstate ^= ( (uint32_t)1 << pin );
    return 1;
  }
  return 0;
}

int Machine::msgRead( uint8_t id_msg ) // Checks msg queue and removes 1 msg
{
  if ( msg_table[id_msg] > 0 ) {
      msg_table[id_msg]--;
      return 1;
  }
  return 0;
}

int Machine::msgRead( uint8_t id_msg, int cnt )
{
  if ( msg_table[id_msg] > 0 ) {
    if ( cnt >= msg_table[id_msg] ) {
        msg_table[id_msg] = 0;
    } else {
        msg_table[id_msg] -= cnt;
    }
    return 1;
  }
  return 0;
}

int Machine::msgRead( uint8_t id_msg, int cnt, int clear )
{
  if ( msg_table[id_msg] > 0 ) {
    if ( cnt >= msg_table[id_msg] ) {
        msg_table[id_msg] = 0;
    } else {
        msg_table[id_msg] -= cnt;
    }
	if ( clear ) {
		for ( int i = 0; i < msg_width; i++ ) {
			msg_table[i] = 0;
		}
    }
    return 1;
  }
  return 0;
}

int Machine::msgPeek( uint8_t id_msg )
{
  return msg_table[id_msg];
}

int Machine::msgClear( uint8_t id_msg ) // Destructive read (clears queue for this type)
{
  flags &= ~ATM_SLEEP_FLAG;
  if ( msg_table[id_msg] > 0 ) {
    msg_table[id_msg] = 0;
    return 1;
  }
  return 0;
}

Machine & Machine::msgClear()
{
  flags &= ~ATM_SLEEP_FLAG;
  for ( int i = 0; i < msg_width; i++ ) {
    msg_table[i] = 0;
  }
  return *this;
}

Machine & Machine::msgWrite( uint8_t id_msg )
{
  flags &= ~ATM_SLEEP_FLAG;
  msg_table[id_msg]++;
  return *this;
}

Machine & Machine::msgWrite( uint8_t id_msg, int cnt )
{
  flags &= ~ATM_SLEEP_FLAG;
  msg_table[id_msg] += cnt;
  return *this;
}

const char * Machine::map_symbol( int id, const char map[] )
{
  int cnt = 0;
  int i = 0;
  if ( id == -1 ) return "*NONE*";
  if ( id ==  0 ) return map;
  while ( 1 ) {
    if ( map[i] == '\0' && ++cnt == id ) {
      i++;
      break;
    }
    i++;
  }
  return &map[i];
}

// .cycle() Executes one cycle of a state machine
Machine & Machine::cycle()
{
    if ( ( flags & ATM_SLEEP_FLAG ) == 0 ) {
        cycles++;
        if ( next != -1 ) {
            action( ATM_ON_SWITCH );
            if ( callback_sym ) {
                callback_sym( inst_label,
                    map_symbol(      current, sym_states ),
                    map_symbol(         next, sym_states ),
                    map_symbol( last_trigger, sym_events ), millis() - state_millis, cycles );
            }
            if ( current > -1 )
		action( read_state( state_table + ( current * state_width ) + ATM_ON_EXIT ) );
            previous = current;
            current = next;
            next = -1;
            state_millis = millis();
            state_micros = micros();
            action( read_state( state_table + ( current * state_width ) + ATM_ON_ENTER ) );
            if ( read_state( state_table + ( current * state_width ) + ATM_ON_LOOP ) == ATM_SLEEP ) {
                  flags |= ATM_SLEEP_FLAG;
            } else {
                  flags &= ~ATM_SLEEP_FLAG;
            }
            cycles = 0;
        }
        state_t i = read_state( state_table + ( current * state_width ) + ATM_ON_LOOP );
        if ( i != -1 ) { action( i ); }
        for ( i = ATM_ON_EXIT + 1; i < state_width; i++ ) {
            if ( ( read_state( state_table + ( current * state_width ) + i ) != -1 ) && ( i == state_width - 1 || event( i - ATM_ON_EXIT - 1 ) ) ) {
                state( read_state( state_table + ( current * state_width ) + i ) );
                last_trigger = i - ATM_ON_EXIT - 1;
                return *this;
            }
        }
    }
    return *this;
}

// TINY MACHINE

TinyMachine & TinyMachine::state(tiny_state_t state)
{
    next = state;
    flags &= ~ATM_SLEEP_FLAG;
    return *this;
}

tiny_state_t TinyMachine::state()
{
    return current;
}

int TinyMachine::trigger( int evt )
{
    if ( current == -1 ) cycle();
    if ( current > -1 ) {
        int new_state = tiny_read_state( state_table + ( current * state_width ) + evt + ATM_ON_EXIT + 1 );
        if ( new_state > -1 ) {
          state( new_state );
          return 1;
        }
    }
    return 0;
}

TinyMachine & TinyMachine::begin( const tiny_state_t* tbl, int width )
{
    state_table = tbl;
    state_width = ATM_ON_EXIT + width + 2;
    return *this;
}

// .cycle() Executes one cycle of a state machine
TinyMachine & TinyMachine::cycle()
{
    if ( ( flags & ATM_SLEEP_FLAG ) == 0 ) {
        if ( next != -1 ) {
            action( ATM_ON_SWITCH );
            if ( current > -1 )
                action( tiny_read_state( state_table + ( current * state_width ) + ATM_ON_EXIT ) );
            current = next;
            next = -1;
            state_millis = millis();
            state_micros = micros();
            action( tiny_read_state( state_table + ( current * state_width ) + ATM_ON_ENTER ) );
            if ( read_state( state_table + ( current * state_width ) + ATM_ON_LOOP ) == ATM_SLEEP ) {
                  flags |= ATM_SLEEP_FLAG;
            } else {
                  flags &= ~ATM_SLEEP_FLAG;
            }
        }
        tiny_state_t i = tiny_read_state( state_table + ( current * state_width ) + ATM_ON_LOOP );
        if ( i != -1 ) { action( i ); }
        for ( i = ATM_ON_EXIT + 1; i < state_width; i++ ) {
            if ( ( tiny_read_state( state_table + ( current * state_width ) + i ) != -1 ) && ( i == state_width - 1 || event( i - ATM_ON_EXIT - 1 ) ) ) {
                state( tiny_read_state( state_table + ( current * state_width ) + i ) );
                return *this;
            }
        }
    }
    return *this;
}

// FACTORY

// .calibrate() Distributes the machines in the inventory to the appropriate priority queues
void Factory::calibrate( void )
{
    // Reset all priority queues to empty lists
    for ( int8_t i = 0; i < ATM_NO_OF_QUEUES; i++ )
        priority_root[i] = 0;
    // Walk the inventory list that contains all state machines in this factory
    Machine * m = inventory_root;
    while ( m ) {
        // Prepend every machine to the appropriate priority queue
        if ( m->prio < ATM_NO_OF_QUEUES ) {
            m->priority_next = priority_root[m->prio];
            priority_root[m->prio] = m;
        }
        m = m->inventory_next;
    }
    recalibrate = 0;
}

// .run( q ) Traverses an individual priority queue and cycles the machines in it once (except for queue 0)
void Factory::run( int q )
{
    Machine * m = priority_root[ q ];
    while ( m ) {
        if ( q > 0 && ( m->flags & ATM_SLEEP_FLAG ) == 0 ) m->cycle();
        // Request a recalibrate if the prio field doesn't match the current queue
        if ( m->prio != q ) recalibrate = 1;
        // Move to the next machine
        m = m->priority_next;
    }
}

// .add( machine ) Adds a state machine to the factory by prepending it to the inventory list
Factory & Factory::add( Machine & machine )
{
    machine.inventory_next = inventory_root;
    inventory_root = &machine;
    machine.factory = this;
    recalibrate = 1;
    return *this;
}

// .find() Search the factory inventory for a machine by instance label
Machine * Factory::find( const char label[] )
{
    Machine * m = inventory_root;
    while ( m ) {
        if ( strcmp( label, m->inst_label ) == 0 ) {
            return m;
        }
        m = m->inventory_next;
    }
    return 0;
}

int Factory::msgSend( const char label[], int msg, int cnt )
{
    int r = 0;
    Machine * m = inventory_root;
    while ( m ) {
        if ( strcmp( label, m->inst_label ) == 0 ) {
            m->msgWrite( msg, cnt );
            r++;
        }
        m = m->inventory_next;
    }
    return r;
}

int Factory::msgSend( const char label[], int msg )
{
    msgSend( label, msg, 1 );
}

int Factory::msgSendClass( const char label[], int msg, int cnt )
{
    int r = 0;
    Machine * m = inventory_root;
    while ( m ) {
        if ( strcmp( label, m->class_label ) == 0 ) {
            m->msgWrite( msg, cnt );
            r++;
        }
        m = m->inventory_next;
    }
    return r;
}

int Factory::msgSendClass( const char label[], int msg )
{
    msgSendClass( label, msg, 1 );
}

// .cycle() executes one factory cycle (runs all priority queues a certain number of times)
Factory & Factory::cycle( void )
{
    if ( recalibrate ) calibrate();
    run( 1 ); run( 2 );	run( 1 ); run( 2 );
    run( 1 ); run( 3 );	run( 1 ); run( 4 );
    run( 1 ); run( 2 );	run( 1 ); run( 3 );
    run( 1 ); run( 2 );	run( 1 ); run( 0 );
    return  *this;
}

<|endoftext|>


<commit_before>/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010-2011, 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 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.
 *
 * $Id$
 */

#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/console/print.h>
#include <pcl/console/parse.h>
#include <pcl/console/time.h>
#include <pcl/kdtree/kdtree_flann.h>

using namespace pcl;
using namespace pcl::io;
using namespace pcl::console;

std::string default_correspondence_type = "index";

void
printHelp (int argc, char **argv)
{
  print_error ("Syntax is: %s source.pcd target.pcd output_intensity.pcd <options>\n", argv[0]);
  print_info ("  where options are:\n");
  print_info ("                     -correspondence X = the way of selecting the corresponding pair in the target cloud for the current point in the source cloud\n");
  print_info ("                                         options are: index = points with identical indices are paired together. Note: both clouds need to have the same number of points\n");
  print_info ("                                                      nn = source point is paired with its nearest neighbor in the target cloud\n");
  print_info ("                                                      nnplane = source point is paired with its projection on the plane determined by the nearest neighbor in the target cloud. Note: target cloud needs to contain normals\n");
  print_info ("                                         (default: ");
  print_value ("%s", default_correspondence_type.c_str ()); print_info (")\n");
}

bool
loadCloud (const std::string &filename, sensor_msgs::PointCloud2 &cloud)
{
  TicToc tt;
  print_highlight ("Loading "); print_value ("%s ", filename.c_str ());

  tt.tic ();
  if (loadPCDFile (filename, cloud) < 0)
    return (false);
  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud.width * cloud.height); print_info (" points]\n");
  print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (cloud).c_str ());

  return (true);
}

void
compute (const sensor_msgs::PointCloud2::ConstPtr &cloud_source, const sensor_msgs::PointCloud2::ConstPtr &cloud_target,
         sensor_msgs::PointCloud2 &output, std::string correspondence_type)
{
  // Estimate
  TicToc tt;
  tt.tic ();

  PointCloud<PointXYZ>::Ptr xyz_source (new PointCloud<PointXYZ> ());
  fromROSMsg (*cloud_source, *xyz_source);
  PointCloud<PointXYZ>::Ptr xyz_target (new PointCloud<PointXYZ> ());
  fromROSMsg (*cloud_target, *xyz_target);

  PointCloud<PointXYZI>::Ptr output_xyzi (new PointCloud<PointXYZI> ());
  output_xyzi->points.resize (xyz_source->points.size ());
  output_xyzi->height = cloud_source->height;
  output_xyzi->width = cloud_source->width;

  float rmse = 0.0f;

  if (correspondence_type == "index")
  {
    print_highlight (stderr, "Computing using the equal indices correspondence heuristic.\n");

    if (xyz_source->points.size () != xyz_target->points.size ())
    {
      print_error ("Source and target clouds do not have the same number of points.\n");
      return;
    }

    for (size_t point_i = 0; point_i < xyz_source->points.size (); ++point_i)
    {
      if (!pcl_isfinite (xyz_source->points[point_i].x) || !pcl_isfinite (xyz_source->points[point_i].y) || !pcl_isfinite (xyz_source->points[point_i].z))
        continue;
      if (!pcl_isfinite (xyz_target->points[point_i].x) || !pcl_isfinite (xyz_target->points[point_i].y) || !pcl_isfinite (xyz_target->points[point_i].z))
        continue;

      float dist = squaredEuclideanDistance (xyz_source->points[point_i], xyz_target->points[point_i]);
      rmse += dist;

      output_xyzi->points[point_i].x = xyz_source->points[point_i].x;
      output_xyzi->points[point_i].y = xyz_source->points[point_i].y;
      output_xyzi->points[point_i].z = xyz_source->points[point_i].z;
      output_xyzi->points[point_i].intensity = dist;
    }
    rmse = sqrt (rmse / xyz_source->points.size ());
  }
  else if (correspondence_type == "nn")
  {
    print_highlight (stderr, "Computing using the nearest neighbor correspondence heuristic.\n");

    KdTreeFLANN<PointXYZ>::Ptr tree (new KdTreeFLANN<PointXYZ> ());
    tree->setInputCloud (xyz_target);

    for (size_t point_i = 0; point_i < xyz_source->points.size (); ++ point_i)
    {
      if (!pcl_isfinite (xyz_source->points[point_i].x) || !pcl_isfinite (xyz_source->points[point_i].y) || !pcl_isfinite (xyz_source->points[point_i].z))
        continue;

      std::vector<int> nn_indices (1);
      std::vector<float> nn_distances (1);
      if (!tree->nearestKSearch (point_i, 1, nn_indices, nn_distances))
        continue;
      size_t point_nn_i = nn_indices.front();

      float dist = squaredEuclideanDistance (xyz_source->points[point_i], xyz_target->points[point_nn_i]);
      rmse += dist;

      output_xyzi->points[point_i].x = xyz_source->points[point_i].x;
      output_xyzi->points[point_i].y = xyz_source->points[point_i].y;
      output_xyzi->points[point_i].z = xyz_source->points[point_i].z;
      output_xyzi->points[point_i].intensity = dist;
    }
    rmse = sqrt (rmse / xyz_source->points.size ());

  }
  else if (correspondence_type == "nnplane")
  {
    print_highlight (stderr, "Computing using the nearest neighbor plane projection correspondence heuristic.\n");

    PointCloud<Normal>::Ptr normals_target (new PointCloud<Normal> ());
    fromROSMsg (*cloud_target, *normals_target);

    KdTreeFLANN<PointXYZ>::Ptr tree (new KdTreeFLANN<PointXYZ> ());
    tree->setInputCloud (xyz_target);

    for (size_t point_i = 0; point_i < xyz_source->points.size (); ++ point_i)
    {
      if (!pcl_isfinite (xyz_source->points[point_i].x) || !pcl_isfinite (xyz_source->points[point_i].y) || !pcl_isfinite (xyz_source->points[point_i].z))
        continue;

      std::vector<int> nn_indices (1);
      std::vector<float> nn_distances (1);
      if (!tree->nearestKSearch (point_i, 1, nn_indices, nn_distances))
        continue;
      size_t point_nn_i = nn_indices.front();

      Eigen::Vector3f normal_target = normals_target->points[point_nn_i].getNormalVector3fMap (),
          point_source = xyz_source->points[point_i].getVector3fMap (),
          point_target = xyz_target->points[point_nn_i].getVector3fMap ();

      float dist = normal_target.dot (point_source - point_target);
      rmse += dist * dist;

      output_xyzi->points[point_i].x = xyz_source->points[point_i].x;
      output_xyzi->points[point_i].y = xyz_source->points[point_i].y;
      output_xyzi->points[point_i].z = xyz_source->points[point_i].z;
      output_xyzi->points[point_i].intensity = dist * dist;
    }
    rmse = sqrt (rmse / xyz_source->points.size ());
  }
  else
  {
    print_error ("Unrecognized correspondence type. Check legal arguments by using the -h option\n");
    return;
  }

  toROSMsg (*output_xyzi, output);

  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms]\n");
  print_highlight ("RMSE Error: %f\n", rmse);
}

void
saveCloud (const std::string &filename, const sensor_msgs::PointCloud2 &output)
{
  TicToc tt;
  tt.tic ();

  print_highlight ("Saving "); print_value ("%s ", filename.c_str ());

  pcl::io::savePCDFile (filename, output);

  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output.width * output.height); print_info (" points]\n");
}

/* ---[ */
int
main (int argc, char** argv)
{
  print_info ("Compute the differences between two point clouds and visualizing them as an output intensity cloud. For more information, use: %s -h\n", argv[0]);

  if (argc < 4)
  {
    printHelp (argc, argv);
    return (-1);
  }

  // Parse the command line arguments for .pcd files
  std::vector<int> p_file_indices;
  p_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
  if (p_file_indices.size () != 3)
  {
    print_error ("Need two input PCD files and one output PCD file to continue.\n");
    return (-1);
  }

  // Command line parsing
  std::string correspondence_type = default_correspondence_type;
  parse_argument (argc, argv, "-correspondence", correspondence_type);

  // Load the first file
  sensor_msgs::PointCloud2::Ptr cloud_source (new sensor_msgs::PointCloud2 ());
  if (!loadCloud (argv[p_file_indices[0]], *cloud_source))
    return (-1);
  // Load the second file
  sensor_msgs::PointCloud2::Ptr cloud_target (new sensor_msgs::PointCloud2 ());
  if (!loadCloud (argv[p_file_indices[1]], *cloud_target))
    return (-1);

  sensor_msgs::PointCloud2 output;
  // Perform the feature estimation
  compute (cloud_source, cloud_target, output, correspondence_type);

  // Output the third file
  saveCloud (argv[p_file_indices[2]], output);
}
<commit_msg>tools/compute_cloud_error bug fix<commit_after>/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010-2011, 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 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.
 *
 * $Id$
 */

#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/console/print.h>
#include <pcl/console/parse.h>
#include <pcl/console/time.h>
#include <pcl/kdtree/kdtree_flann.h>

using namespace pcl;
using namespace pcl::io;
using namespace pcl::console;

std::string default_correspondence_type = "index";

void
printHelp (int argc, char **argv)
{
  print_error ("Syntax is: %s source.pcd target.pcd output_intensity.pcd <options>\n", argv[0]);
  print_info ("  where options are:\n");
  print_info ("                     -correspondence X = the way of selecting the corresponding pair in the target cloud for the current point in the source cloud\n");
  print_info ("                                         options are: index = points with identical indices are paired together. Note: both clouds need to have the same number of points\n");
  print_info ("                                                      nn = source point is paired with its nearest neighbor in the target cloud\n");
  print_info ("                                                      nnplane = source point is paired with its projection on the plane determined by the nearest neighbor in the target cloud. Note: target cloud needs to contain normals\n");
  print_info ("                                         (default: ");
  print_value ("%s", default_correspondence_type.c_str ()); print_info (")\n");
}

bool
loadCloud (const std::string &filename, sensor_msgs::PointCloud2 &cloud)
{
  TicToc tt;
//  print_highlight ("Loading "); print_value ("%s ", filename.c_str ());

  tt.tic ();
  if (loadPCDFile (filename, cloud) < 0)
    return (false);
//  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" seconds : "); print_value ("%d", cloud.width * cloud.height); print_info (" points]\n");
//  print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (cloud).c_str ());

  return (true);
}

void
compute (const sensor_msgs::PointCloud2::ConstPtr &cloud_source, const sensor_msgs::PointCloud2::ConstPtr &cloud_target,
         sensor_msgs::PointCloud2 &output, std::string correspondence_type)
{
  // Estimate
  TicToc tt;
  tt.tic ();

  PointCloud<PointXYZ>::Ptr xyz_source (new PointCloud<PointXYZ> ());
  fromROSMsg (*cloud_source, *xyz_source);
  PointCloud<PointXYZ>::Ptr xyz_target (new PointCloud<PointXYZ> ());
  fromROSMsg (*cloud_target, *xyz_target);

  PointCloud<PointXYZI>::Ptr output_xyzi (new PointCloud<PointXYZI> ());
  output_xyzi->points.resize (xyz_source->points.size ());
  output_xyzi->height = cloud_source->height;
  output_xyzi->width = cloud_source->width;

  float rmse = 0.0f;

  if (correspondence_type == "index")
  {
//    print_highlight (stderr, "Computing using the equal indices correspondence heuristic.\n");

    if (xyz_source->points.size () != xyz_target->points.size ())
    {
      print_error ("Source and target clouds do not have the same number of points.\n");
      return;
    }

    for (size_t point_i = 0; point_i < xyz_source->points.size (); ++point_i)
    {
      if (!pcl_isfinite (xyz_source->points[point_i].x) || !pcl_isfinite (xyz_source->points[point_i].y) || !pcl_isfinite (xyz_source->points[point_i].z))
        continue;
      if (!pcl_isfinite (xyz_target->points[point_i].x) || !pcl_isfinite (xyz_target->points[point_i].y) || !pcl_isfinite (xyz_target->points[point_i].z))
        continue;

      float dist = squaredEuclideanDistance (xyz_source->points[point_i], xyz_target->points[point_i]);
      rmse += dist;

      output_xyzi->points[point_i].x = xyz_source->points[point_i].x;
      output_xyzi->points[point_i].y = xyz_source->points[point_i].y;
      output_xyzi->points[point_i].z = xyz_source->points[point_i].z;
      output_xyzi->points[point_i].intensity = dist;
    }
    rmse = sqrt (rmse / xyz_source->points.size ());
  }
  else if (correspondence_type == "nn")
  {
//    print_highlight (stderr, "Computing using the nearest neighbor correspondence heuristic.\n");

    KdTreeFLANN<PointXYZ>::Ptr tree (new KdTreeFLANN<PointXYZ> ());
    tree->setInputCloud (xyz_target);

    for (size_t point_i = 0; point_i < xyz_source->points.size (); ++ point_i)
    {
      if (!pcl_isfinite (xyz_source->points[point_i].x) || !pcl_isfinite (xyz_source->points[point_i].y) || !pcl_isfinite (xyz_source->points[point_i].z))
        continue;

      std::vector<int> nn_indices (1);
      std::vector<float> nn_distances (1);
      if (!tree->nearestKSearch (xyz_source->points[point_i], 1, nn_indices, nn_distances))
        continue;
      size_t point_nn_i = nn_indices.front();

      float dist = squaredEuclideanDistance (xyz_source->points[point_i], xyz_target->points[point_nn_i]);
      rmse += dist;

      output_xyzi->points[point_i].x = xyz_source->points[point_i].x;
      output_xyzi->points[point_i].y = xyz_source->points[point_i].y;
      output_xyzi->points[point_i].z = xyz_source->points[point_i].z;
      output_xyzi->points[point_i].intensity = dist;
    }
    rmse = sqrt (rmse / xyz_source->points.size ());

  }
  else if (correspondence_type == "nnplane")
  {
//    print_highlight (stderr, "Computing using the nearest neighbor plane projection correspondence heuristic.\n");

    PointCloud<Normal>::Ptr normals_target (new PointCloud<Normal> ());
    fromROSMsg (*cloud_target, *normals_target);

    KdTreeFLANN<PointXYZ>::Ptr tree (new KdTreeFLANN<PointXYZ> ());
    tree->setInputCloud (xyz_target);

    for (size_t point_i = 0; point_i < xyz_source->points.size (); ++ point_i)
    {
      if (!pcl_isfinite (xyz_source->points[point_i].x) || !pcl_isfinite (xyz_source->points[point_i].y) || !pcl_isfinite (xyz_source->points[point_i].z))
        continue;

      std::vector<int> nn_indices (1);
      std::vector<float> nn_distances (1);
      if (!tree->nearestKSearch (xyz_source->points[point_i], 1, nn_indices, nn_distances))
        continue;
      size_t point_nn_i = nn_indices.front();

      Eigen::Vector3f normal_target = normals_target->points[point_nn_i].getNormalVector3fMap (),
          point_source = xyz_source->points[point_i].getVector3fMap (),
          point_target = xyz_target->points[point_nn_i].getVector3fMap ();

      float dist = normal_target.dot (point_source - point_target);
      rmse += dist * dist;

      output_xyzi->points[point_i].x = xyz_source->points[point_i].x;
      output_xyzi->points[point_i].y = xyz_source->points[point_i].y;
      output_xyzi->points[point_i].z = xyz_source->points[point_i].z;
      output_xyzi->points[point_i].intensity = dist * dist;
    }
    rmse = sqrt (rmse / xyz_source->points.size ());
  }
  else
  {
//    print_error ("Unrecognized correspondence type. Check legal arguments by using the -h option\n");
    return;
  }

  toROSMsg (*output_xyzi, output);

//  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" seconds]\n");
  print_highlight ("RMSE Error: %f\n", rmse);
}

void
saveCloud (const std::string &filename, const sensor_msgs::PointCloud2 &output)
{
  TicToc tt;
  tt.tic ();

//  print_highlight ("Saving "); print_value ("%s ", filename.c_str ());

  pcl::io::savePCDFile (filename, output);

//  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" seconds : "); print_value ("%d", output.width * output.height); print_info (" points]\n");
}

/* ---[ */
int
main (int argc, char** argv)
{
//  print_info ("Compute the differences between two point clouds and visualizing them as an output intensity cloud. For more information, use: %s -h\n", argv[0]);

  if (argc < 4)
  {
    printHelp (argc, argv);
    return (-1);
  }

  // Parse the command line arguments for .pcd files
  std::vector<int> p_file_indices;
  p_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
  if (p_file_indices.size () != 3)
  {
    print_error ("Need two input PCD files and one output PCD file to continue.\n");
    return (-1);
  }

  // Command line parsing
  std::string correspondence_type = default_correspondence_type;
  parse_argument (argc, argv, "-correspondence", correspondence_type);

  // Load the first file
  sensor_msgs::PointCloud2::Ptr cloud_source (new sensor_msgs::PointCloud2 ());
  if (!loadCloud (argv[p_file_indices[0]], *cloud_source))
    return (-1);
  // Load the second file
  sensor_msgs::PointCloud2::Ptr cloud_target (new sensor_msgs::PointCloud2 ());
  if (!loadCloud (argv[p_file_indices[1]], *cloud_target))
    return (-1);

  sensor_msgs::PointCloud2 output;
  // Perform the feature estimation
  compute (cloud_source, cloud_target, output, correspondence_type);

  // Output the third file
  saveCloud (argv[p_file_indices[2]], output);
}
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2013, Hernan Saez
 * 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 <organization> 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 <COPYRIGHT HOLDER> 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.
 */

#ifndef CRIMILD_CORE_SIMULATION_ASSET_MANAGER_
#define CRIMILD_CORE_SIMULATION_ASSET_MANAGER_

#include "Foundation/SharedObject.hpp"

#include <memory>
#include <map>
#include <string>

namespace crimild {

    class AssetManager {
    public:
        AssetManager( void );
        virtual ~AssetManager( void );

        void add( std::string name, SharedObjectPtr const &asset )
        {
            _assets[ name ] = asset;
        }

        template< class T >
        SharedPointer< T > get( std::string name )
        {
            return std::static_pointer_cast< T >( _assets[ name ] );
        }

        void clear( void )
        {
            _assets.clear();
        }

    private:
        std::map< std::string, SharedObjectPtr > _assets;
    };

}

#endif

<commit_msg>Disallow copy and assign for AssetManager<commit_after>/*
 * Copyright (c) 2013, Hernan Saez
 * 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 <organization> 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 <COPYRIGHT HOLDER> 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.
 */

#ifndef CRIMILD_CORE_SIMULATION_ASSET_MANAGER_
#define CRIMILD_CORE_SIMULATION_ASSET_MANAGER_

#include "Foundation/SharedObject.hpp"
#include "Foundation/Macros.hpp"

#include <memory>
#include <map>
#include <string>

namespace crimild {

    class AssetManager {
        CRIMILD_DISALLOW_COPY_AND_ASSIGN( AssetManager )

    public:
        AssetManager( void );
        virtual ~AssetManager( void );

        void add( std::string name, SharedObjectPtr const &asset )
        {
            _assets[ name ] = asset;
        }

        template< class T >
        SharedPointer< T > get( std::string name )
        {
            return std::static_pointer_cast< T >( _assets[ name ] );
        }

        void clear( void )
        {
            _assets.clear();
        }

    private:
        std::map< std::string, SharedObjectPtr > _assets;
    };

}

#endif

<|endoftext|>


<commit_before>// @(#)root/thread:$Id$
// Authors: Enric Tejedor CERN  12/09/2016
//          Philippe Canal FNAL 12/09/2016

/*************************************************************************
 * Copyright (C) 1995-2016, Rene Brun and Fons Rademakers.               *
 * All rights reserved.                                                  *
 *                                                                       *
 * For the licensing terms see $ROOTSYS/LICENSE.                         *
 * For the list of contributors see $ROOTSYS/README/CREDITS.             *
 *************************************************************************/

/** \class TReentrantRWLock
    \brief An implementation of a reentrant read-write lock with a
           configurable internal mutex/lock (default Spin Lock).

This class provides an implementation of a rreentrant ead-write lock
that uses an internal lock and a condition variable to synchronize
readers and writers when necessary.

The implementation allows a single reader to take the write lock without
releasing the reader lock.  It also allows the writer to take a read lock.
In other word, the lock is re-entrant for both reading and writing.

The implementation tries to make faster the scenario when readers come
and go but there is no writer. In that case, readers will not pay the
price of taking the internal spin lock.

Moreover, this RW lock tries to be fair with writers, giving them the
possibility to claim the lock and wait for only the remaining readers,
thus preventing starvation.
*/

#include "ROOT/TReentrantRWLock.hxx"
#include "ROOT/TSpinMutex.hxx"
#include "TMutex.h"
#include "TError.h"

using namespace ROOT;

Internal::UniqueLockRecurseCount::UniqueLockRecurseCount()
{
   static bool singleton = false;
   if (singleton) {
      ::Fatal("UniqueLockRecurseCount Ctor", "Only one TReentrantRWLock using a UniqueLockRecurseCount is allowed.");
   }
   singleton = true;
}

////////////////////////////////////////////////////////////////////////////
/// Acquire the lock in read mode.
template <typename MutexT, typename RecurseCountsT>
void TReentrantRWLock<MutexT, RecurseCountsT>::ReadLock()
{
   ++fReaderReservation;

   // if (fReaders == std::numeric_limits<decltype(fReaders)>::max()) {
   //    ::Fatal("TRWSpinLock::WriteLock", "Too many recursions in TRWSpinLock!");
   // }

   auto local = fRecurseCounts.GetLocal();

   if (!fWriter) {
      // There is no writer, go freely to the critical section
      ++fReaders;
      --fReaderReservation;

      fRecurseCounts.IncrementReadCount(local, fMutex);
   } else {
      // A writer claimed the RW lock, we will need to wait on the
      // internal lock
      --fReaderReservation;

      std::unique_lock<MutexT> lock(fMutex);

      // Wait for writers, if any
      if (fWriter && fRecurseCounts.IsNotCurrentWriter(local)) fCond.wait(lock, [this] { return !fWriter; });

      fRecurseCounts.IncrementReadCount(local);

      // This RW lock now belongs to the readers
      ++fReaders;

      lock.unlock();
   }
}

//////////////////////////////////////////////////////////////////////////
/// Release the lock in read mode.
template <typename MutexT, typename RecurseCountsT>
void TReentrantRWLock<MutexT, RecurseCountsT>::ReadUnLock()
{
   auto local = fRecurseCounts.GetLocal();

   --fReaders;
   if (fWriterReservation && fReaders == 0) {
      // We still need to lock here to prevent interleaving with a writer
      std::lock_guard<MutexT> lock(fMutex);

      fRecurseCounts.DecrementReadCount(local);

      // Make sure you wake up a writer, if any
      // Note: spurrious wakeups are okay, fReaders
      // will be checked again in WriteLock
      fCond.notify_all();
   } else {

      fRecurseCounts.DecrementReadCount(local, fMutex);
   }
}

//////////////////////////////////////////////////////////////////////////
/// Acquire the lock in write mode.
template <typename MutexT, typename RecurseCountsT>
void TReentrantRWLock<MutexT, RecurseCountsT>::WriteLock()
{
   ++fWriterReservation;

   std::unique_lock<MutexT> lock(fMutex);

   auto local = fRecurseCounts.GetLocal();

   // Release this thread's reader lock(s)
   auto readerCount = fRecurseCounts.GetLocalReadersCount(local);

   fReaders -= readerCount;

   // Wait for other writers, if any
   if (fWriter && fRecurseCounts.IsNotCurrentWriter(local)) {
      if (readerCount && fReaders == 0) {
         // we decrease fReaders to zero, let's wake up the
         // other writer.
         fCond.notify_all();
      }
      fCond.wait(lock, [this] { return !fWriter; });
   }

   // Claim the lock for this writer
   fWriter = true;
   fRecurseCounts.SetIsWriter(local);

   // Wait until all reader reservations finish
   while (fReaderReservation) {
   };

   // Wait for remaining readers
   fCond.wait(lock, [this] { return fReaders == 0; });

   // Restore this thread's reader lock(s)
   fReaders += readerCount;

   --fWriterReservation;

   lock.unlock();
}

//////////////////////////////////////////////////////////////////////////
/// Release the lock in write mode.
template <typename MutexT, typename RecurseCountsT>
void TReentrantRWLock<MutexT, RecurseCountsT>::WriteUnLock()
{
   // We need to lock here to prevent interleaving with a reader
   std::lock_guard<MutexT> lock(fMutex);

   if (!fWriter || fRecurseCounts.fWriteRecurse == 0) {
      Error("TReentrantRWLock::WriteUnLock", "Write lock already released for %p", this);
      return;
   }

   fRecurseCounts.DecrementWriteCount();

   if (!fRecurseCounts.fWriteRecurse) {
      fWriter = false;

      auto local = fRecurseCounts.GetLocal();
      fRecurseCounts.ResetIsWriter(local);

      // Notify all potential readers/writers that are waiting
      fCond.notify_all();
   }
}

namespace {
template <typename MutexT, typename RecurseCountsT>
struct TReentrantRWLockState: public TVirtualMutex::State {
   RecurseCountsT fRecurseCounts;
};
}

template <typename MutexT, typename RecurseCountsT>
std::unique_ptr<TVirtualMutex::State> TReentrantRWLock<MutexT, RecurseCountsT>::Reset()
{
   std::unique_ptr<TReentrantRWLockState<MutexT, RecurseCountsT>> pState;
   // Do something.
   ::Fatal("Reset()", "Not implemented, contact pcanal@fnal.gov");
   return std::move(pState);
}

template <typename MutexT, typename RecurseCountsT>
void TReentrantRWLock<MutexT, RecurseCountsT>::Restore(std::unique_ptr<TVirtualMutex::State> &&)
{
   // Do something.
   ::Fatal("Restore()", "Not implemented, contact pcanal@fnal.gov");
}

namespace ROOT {
template class TReentrantRWLock<ROOT::TSpinMutex, ROOT::Internal::RecurseCounts>;
template class TReentrantRWLock<TMutex, ROOT::Internal::RecurseCounts>;

template class TReentrantRWLock<ROOT::TSpinMutex, ROOT::Internal::UniqueLockRecurseCount>;
template class TReentrantRWLock<TMutex, ROOT::Internal::UniqueLockRecurseCount>;
}
<commit_msg>Implement TReentrantRWLock::Reset/Restore<commit_after>// @(#)root/thread:$Id$
// Authors: Enric Tejedor CERN  12/09/2016
//          Philippe Canal FNAL 12/09/2016

/*************************************************************************
 * Copyright (C) 1995-2016, Rene Brun and Fons Rademakers.               *
 * All rights reserved.                                                  *
 *                                                                       *
 * For the licensing terms see $ROOTSYS/LICENSE.                         *
 * For the list of contributors see $ROOTSYS/README/CREDITS.             *
 *************************************************************************/

/** \class TReentrantRWLock
    \brief An implementation of a reentrant read-write lock with a
           configurable internal mutex/lock (default Spin Lock).

This class provides an implementation of a rreentrant ead-write lock
that uses an internal lock and a condition variable to synchronize
readers and writers when necessary.

The implementation allows a single reader to take the write lock without
releasing the reader lock.  It also allows the writer to take a read lock.
In other word, the lock is re-entrant for both reading and writing.

The implementation tries to make faster the scenario when readers come
and go but there is no writer. In that case, readers will not pay the
price of taking the internal spin lock.

Moreover, this RW lock tries to be fair with writers, giving them the
possibility to claim the lock and wait for only the remaining readers,
thus preventing starvation.
*/

#include "ROOT/TReentrantRWLock.hxx"
#include "ROOT/TSpinMutex.hxx"
#include "TMutex.h"
#include "TError.h"

using namespace ROOT;

Internal::UniqueLockRecurseCount::UniqueLockRecurseCount()
{
   static bool singleton = false;
   if (singleton) {
      ::Fatal("UniqueLockRecurseCount Ctor", "Only one TReentrantRWLock using a UniqueLockRecurseCount is allowed.");
   }
   singleton = true;
}

////////////////////////////////////////////////////////////////////////////
/// Acquire the lock in read mode.
template <typename MutexT, typename RecurseCountsT>
void TReentrantRWLock<MutexT, RecurseCountsT>::ReadLock()
{
   ++fReaderReservation;

   // if (fReaders == std::numeric_limits<decltype(fReaders)>::max()) {
   //    ::Fatal("TRWSpinLock::WriteLock", "Too many recursions in TRWSpinLock!");
   // }

   auto local = fRecurseCounts.GetLocal();

   if (!fWriter) {
      // There is no writer, go freely to the critical section
      ++fReaders;
      --fReaderReservation;

      fRecurseCounts.IncrementReadCount(local, fMutex);
   } else {
      // A writer claimed the RW lock, we will need to wait on the
      // internal lock
      --fReaderReservation;

      std::unique_lock<MutexT> lock(fMutex);

      // Wait for writers, if any
      if (fWriter && fRecurseCounts.IsNotCurrentWriter(local)) fCond.wait(lock, [this] { return !fWriter; });

      fRecurseCounts.IncrementReadCount(local);

      // This RW lock now belongs to the readers
      ++fReaders;

      lock.unlock();
   }
}

//////////////////////////////////////////////////////////////////////////
/// Release the lock in read mode.
template <typename MutexT, typename RecurseCountsT>
void TReentrantRWLock<MutexT, RecurseCountsT>::ReadUnLock()
{
   auto local = fRecurseCounts.GetLocal();

   --fReaders;
   if (fWriterReservation && fReaders == 0) {
      // We still need to lock here to prevent interleaving with a writer
      std::lock_guard<MutexT> lock(fMutex);

      fRecurseCounts.DecrementReadCount(local);

      // Make sure you wake up a writer, if any
      // Note: spurrious wakeups are okay, fReaders
      // will be checked again in WriteLock
      fCond.notify_all();
   } else {

      fRecurseCounts.DecrementReadCount(local, fMutex);
   }
}

//////////////////////////////////////////////////////////////////////////
/// Acquire the lock in write mode.
template <typename MutexT, typename RecurseCountsT>
void TReentrantRWLock<MutexT, RecurseCountsT>::WriteLock()
{
   ++fWriterReservation;

   std::unique_lock<MutexT> lock(fMutex);

   auto local = fRecurseCounts.GetLocal();

   // Release this thread's reader lock(s)
   auto readerCount = fRecurseCounts.GetLocalReadersCount(local);

   fReaders -= readerCount;

   // Wait for other writers, if any
   if (fWriter && fRecurseCounts.IsNotCurrentWriter(local)) {
      if (readerCount && fReaders == 0) {
         // we decrease fReaders to zero, let's wake up the
         // other writer.
         fCond.notify_all();
      }
      fCond.wait(lock, [this] { return !fWriter; });
   }

   // Claim the lock for this writer
   fWriter = true;
   fRecurseCounts.SetIsWriter(local);

   // Wait until all reader reservations finish
   while (fReaderReservation) {
   };

   // Wait for remaining readers
   fCond.wait(lock, [this] { return fReaders == 0; });

   // Restore this thread's reader lock(s)
   fReaders += readerCount;

   --fWriterReservation;

   lock.unlock();
}

//////////////////////////////////////////////////////////////////////////
/// Release the lock in write mode.
template <typename MutexT, typename RecurseCountsT>
void TReentrantRWLock<MutexT, RecurseCountsT>::WriteUnLock()
{
   // We need to lock here to prevent interleaving with a reader
   std::lock_guard<MutexT> lock(fMutex);

   if (!fWriter || fRecurseCounts.fWriteRecurse == 0) {
      Error("TReentrantRWLock::WriteUnLock", "Write lock already released for %p", this);
      return;
   }

   fRecurseCounts.DecrementWriteCount();

   if (!fRecurseCounts.fWriteRecurse) {
      fWriter = false;

      auto local = fRecurseCounts.GetLocal();
      fRecurseCounts.ResetIsWriter(local);

      // Notify all potential readers/writers that are waiting
      fCond.notify_all();
   }
}

namespace {
template <typename MutexT, typename RecurseCountsT>
struct TReentrantRWLockState: public TVirtualMutex::State {
    int fReadersCount = 0;
    size_t fWriteRecurse = 0;
    bool fIsWriter = false;
};
}

template <typename MutexT, typename RecurseCountsT>
std::unique_ptr<TVirtualMutex::State> TReentrantRWLock<MutexT, RecurseCountsT>::Reset()
{
   using State_t = TReentrantRWLockState<MutexT, RecurseCountsT>;

   std::unique_ptr<State_t> pState(new State_t);
   auto local = fRecurseCounts.GetLocal();

   size_t readerCount;
   {
      std::unique_lock<MutexT> lock(fMutex);
      readerCount = fRecurseCounts.GetLocalReadersCount(local);
   }

   pState->fReadersCount = readerCount;

   if (fWriter && !fRecurseCounts.IsNotCurrentWriter(local)) {

      // We are holding the write lock.
      pState->fIsWriter = true;
      pState->fWriteRecurse = fRecurseCounts.fWriteRecurse;

      // Now set the lock (and potential read locks) for immediate release.
      fReaders -= readerCount;
      fRecurseCounts.fWriteRecurse = 1;
      // insertion in the reader count can only happen during a ReadLock
      // which can not execute until we release the write lock, so no
      // need to take the local mutex here.
      fRecurseCounts.ResetReadCount(local, 0);

      // Release this thread's write lock
      WriteUnLock();
   } else if (readerCount) {
      // Now set the lock for release.
      {
         std::unique_lock<MutexT> lock(fMutex);
         fReaders -= (readerCount-1);
         fRecurseCounts.ResetReadCount(local, 1);
      }

      // Release this thread's reader lock(s)
      ReadUnLock();
   }

   // Do something.
   //::Fatal("Reset()", "Not implemented, contact pcanal@fnal.gov");
   return std::move(pState);
}

template <typename MutexT, typename RecurseCountsT>
void TReentrantRWLock<MutexT, RecurseCountsT>::Restore(std::unique_ptr<TVirtualMutex::State> &&state)
{
   TReentrantRWLockState<MutexT, RecurseCountsT> *pState = dynamic_cast<TReentrantRWLockState<MutexT, RecurseCountsT> *>(state.get());
   if (!pState) {
      if (state) {
         SysError("Restore", "LOGIC ERROR - invalid state object!");
         return;
      }
      // No state, do nothing.
      return;
   }

   if (pState->fIsWriter) {
      WriteLock();
      // Now that we go the lock, fix up the recursion count.
      std::unique_lock<MutexT> lock(fMutex);
      fRecurseCounts.fWriteRecurse = pState->fWriteRecurse;
      auto local = fRecurseCounts.GetLocal();
      fRecurseCounts.ResetReadCount(local, pState->fReadersCount);
      fReaders +=  pState->fReadersCount;
   } else {
      ReadLock();
      // Now that we go the read lock, fix up the local recursion count.
      assert( pState->fReadersCount  >= 1 );
      auto readerCount = pState->fReadersCount;

      std::unique_lock<MutexT> lock(fMutex);
      auto local = fRecurseCounts.GetLocal();
      fRecurseCounts.ResetReadCount(local, readerCount);
      fReaders += readerCount - 1;
   }

   // Do something.
   //::Fatal("Restore()", "Not implemented, contact pcanal@fnal.gov");
}

namespace ROOT {
template class TReentrantRWLock<ROOT::TSpinMutex, ROOT::Internal::RecurseCounts>;
template class TReentrantRWLock<TMutex, ROOT::Internal::RecurseCounts>;

template class TReentrantRWLock<ROOT::TSpinMutex, ROOT::Internal::UniqueLockRecurseCount>;
template class TReentrantRWLock<TMutex, ROOT::Internal::UniqueLockRecurseCount>;
}
<|endoftext|>


<commit_before>// Common/StringConvert.cpp

#include "StdAfx.h"

#include "StringConvert.h"
extern "C"
{
int global_use_utf16_conversion = 0;
}

#ifdef LOCALE_IS_UTF8

#ifdef __APPLE_CC__
#define UInt32  macUIn32
#include <CoreFoundation/CoreFoundation.h>
#undef UInt32

UString MultiByteToUnicodeString(const AString &srcString, UINT codePage)
{
  if (!srcString.IsEmpty())
  {
    UString resultString;
    const char * path = &srcString[0];

// FIXME    size_t n = strlen(path);

    CFStringRef cfpath = CFStringCreateWithCString(NULL,path,kCFStringEncodingUTF8);

    if (cfpath)
    {

       CFMutableStringRef cfpath2 = CFStringCreateMutableCopy(NULL,0,cfpath);
       CFRelease(cfpath);
       CFStringNormalize(cfpath2,kCFStringNormalizationFormC);

       size_t n = CFStringGetLength(cfpath2);
       for(size_t i =   0 ; i< n ;i++) {
         UniChar uc = CFStringGetCharacterAtIndex(cfpath2,i);
         resultString += (wchar_t)uc; // FIXME
       }

       CFRelease(cfpath2);

       return resultString;
    }
  }

  UString resultString;
  for (int i = 0; i < srcString.Len(); i++)
    resultString += wchar_t(srcString[i] & 255);

  return resultString;
}

AString UnicodeStringToMultiByte(const UString &srcString, UINT codePage)
{
  if (!srcString.IsEmpty())
  {
    const wchar_t * wcs = &srcString[0];
    char utf8[4096];
    UniChar unipath[4096];

    size_t n = wcslen(wcs);

    for(size_t i =   0 ; i<= n ;i++) {
      unipath[i] = wcs[i];
    }

    CFStringRef cfpath = CFStringCreateWithCharacters(NULL,unipath,n);

    CFMutableStringRef cfpath2 = CFStringCreateMutableCopy(NULL,0,cfpath);
    CFRelease(cfpath);
    CFStringNormalize(cfpath2,kCFStringNormalizationFormD);

    CFStringGetCString(cfpath2,(char *)utf8,4096,kCFStringEncodingUTF8);

    CFRelease(cfpath2);

    return AString(utf8);
  }

  AString resultString;
  for (int i = 0; i < srcString.Len(); i++)
  {
    if (srcString[i] >= 256) resultString += '?';
    else                     resultString += char(srcString[i]);
  }
  return resultString;
}

#else /* __APPLE_CC__ */

#include "UTFConvert.h"

UString MultiByteToUnicodeString(const AString &srcString, UINT codePage)
{
  if ((global_use_utf16_conversion) && (!srcString.IsEmpty()))
  {
    UString resultString;
    bool bret = ConvertUTF8ToUnicode(srcString,resultString);
    if (bret) return resultString;
  }

  UString resultString;
  for (int i = 0; i < srcString.Len(); i++)
    resultString += wchar_t(srcString[i] & 255);

  return resultString;
}

AString UnicodeStringToMultiByte(const UString &srcString, UINT codePage)
{
  if ((global_use_utf16_conversion) && (!srcString.IsEmpty()))
  {
    AString resultString;
    ConvertUnicodeToUTF8(srcString,resultString);
    return resultString;
  }

  AString resultString;
  for (int i = 0; i < srcString.Len(); i++)
  {
    if (srcString[i] >= 256) resultString += '?';
    else                     resultString += char(srcString[i]);
  }
  return resultString;
}

#endif /* __APPLE_CC__ */

#else /* LOCALE_IS_UTF8 */

UString MultiByteToUnicodeString(const AString &srcString, UINT /* codePage */ )
{
#ifdef ENV_HAVE_MBSTOWCS
  if ((global_use_utf16_conversion) && (!srcString.IsEmpty()))
  {
    UString resultString;
    int numChars = mbstowcs(resultString.GetBuf(srcString.Len()),srcString,srcString.Len()+1);
    if (numChars >= 0) {
        resultString.ReleaseBuf_SetEnd(numChars);

#if WCHAR_MAX > 0xffff
      for (int i = numChars; i >= 0; i--) {
        if (resultString[i] > 0xffff) {
          wchar_t c = resultString[i] - 0x10000;
          resultString.Delete(i);
          wchar_t texts[]= { ((c >> 10) & 0x3ff) + 0xd800,  (c & 0x3ff) + 0xdc00 , 0 };
          resultString.Insert(i, texts);
          numChars++;
        }
      }
#endif

      return resultString;
    }
  }
#endif

  UString resultString;
  for (int i = 0; i < srcString.Len(); i++)
    resultString += wchar_t(srcString[i] & 255);

  return resultString;
}

AString UnicodeStringToMultiByte(const UString &src, UINT /* codePage */ )
{
#ifdef ENV_HAVE_WCSTOMBS
#if WCHAR_MAX > 0xffff
  UString srcString(src);
  for (int i = 0; i < srcString.Len(); i++) {
    if ((0xd800 <= srcString[i] && srcString[i] <= 0xdbff) && ((i + 1) < srcString.Len()) &&
        (0xdc00 <= srcString[i + 1] && srcString[i + 1] < 0xE000)) {
      wchar_t c = (((srcString[i] - 0xd800) << 10) | (srcString[i + 1] - 0xdc00)) + 0x10000;
      srcString.Delete(i, 2);
      srcString.Insert(i, c);
    }
  }
#else
  const UString &srcString = src;
#endif

  if ((global_use_utf16_conversion) && (!srcString.IsEmpty()))
  {
    AString resultString;
    int numRequiredBytes = srcString.Len() * 6+1;
    int numChars = wcstombs(resultString.GetBuf(numRequiredBytes),srcString,numRequiredBytes);
    if (numChars >= 0) {
      resultString.ReleaseBuf_SetEnd(numChars);
      return resultString;
    }
  }
#else
  const UString &srcString = src;
#endif

  AString resultString;
  for (int i = 0; i < srcString.Len(); i++)
  {
    if (srcString[i] >= 256) resultString += '?';
    else                     resultString += char(srcString[i]);
  }
  return resultString;
}

#endif /* LOCALE_IS_UTF8 */

void MultiByteToUnicodeString2(UString &dest, const AString &srcString, UINT codePage)
{
  dest = MultiByteToUnicodeString(srcString,codePage);
}

void UnicodeStringToMultiByte2(AString &dest, const UString &srcString, UINT codePage)
{
  dest = UnicodeStringToMultiByte(srcString,codePage);
}

UString MultiByteToUnicodeString3(const AString &srcString, iconv_t convBase)
{
    if (srcString.IsEmpty() || convBase == 0 || convBase == (iconv_t)-1)
    {
        return MultiByteToUnicodeString(srcString, 0);
    }

    UString resultString;
    CFStringRef cfpath;
    const char * path = &srcString[0];

    char out[1024];
    char *outPtr = out;
    size_t outSize = 1024;
    char *inPtr = (char *)&srcString[0];
    size_t inSize = strlen(path);

    iconv(convBase, &inPtr, &inSize, &outPtr, &outSize);
    *outPtr = 0;

    return MultiByteToUnicodeString(out, 0);
}

AString UnicodeStringToMultiByte3(const UString &srcString, iconv_t convBase)
{
    AString utf8 = UnicodeStringToMultiByte(srcString, 0);
    if (convBase == 0 || convBase == (iconv_t)-1) {
        return utf8;
    }

    char out[1024];
    char *outPtr = out;
    size_t outSize = 1024;
    char *inPtr = (char *)&utf8[0];;
    size_t inSize = strlen(utf8);

    iconv(convBase, &inPtr, &inSize, &outPtr, &outSize);
    *outPtr = 0;
    return AString(out);
}
<commit_msg>Replacing chars that cannot be converted with ? sign<commit_after>// Common/StringConvert.cpp

#include "StdAfx.h"

#include "StringConvert.h"
extern "C"
{
int global_use_utf16_conversion = 0;
}

#ifdef LOCALE_IS_UTF8

#ifdef __APPLE_CC__
#define UInt32  macUIn32
#include <CoreFoundation/CoreFoundation.h>
#undef UInt32

UString MultiByteToUnicodeString(const AString &srcString, UINT codePage)
{
  if (!srcString.IsEmpty())
  {
    UString resultString;
    const char * path = &srcString[0];

// FIXME    size_t n = strlen(path);

    CFStringRef cfpath = CFStringCreateWithCString(NULL,path,kCFStringEncodingUTF8);

    if (cfpath)
    {

       CFMutableStringRef cfpath2 = CFStringCreateMutableCopy(NULL,0,cfpath);
       CFRelease(cfpath);
       CFStringNormalize(cfpath2,kCFStringNormalizationFormC);

       size_t n = CFStringGetLength(cfpath2);
       for(size_t i =   0 ; i< n ;i++) {
         UniChar uc = CFStringGetCharacterAtIndex(cfpath2,i);
         resultString += (wchar_t)uc; // FIXME
       }

       CFRelease(cfpath2);

       return resultString;
    }
  }

  UString resultString;
  for (int i = 0; i < srcString.Len(); i++)
    resultString += wchar_t(srcString[i] & 255);

  return resultString;
}

AString UnicodeStringToMultiByte(const UString &srcString, UINT codePage)
{
  if (!srcString.IsEmpty())
  {
    const wchar_t * wcs = &srcString[0];
    char utf8[4096];
    UniChar unipath[4096];

    size_t n = wcslen(wcs);

    for(size_t i =   0 ; i<= n ;i++) {
      unipath[i] = wcs[i];
    }

    CFStringRef cfpath = CFStringCreateWithCharacters(NULL,unipath,n);

    CFMutableStringRef cfpath2 = CFStringCreateMutableCopy(NULL,0,cfpath);
    CFRelease(cfpath);
    CFStringNormalize(cfpath2,kCFStringNormalizationFormD);

    CFStringGetCString(cfpath2,(char *)utf8,4096,kCFStringEncodingUTF8);

    CFRelease(cfpath2);

    return AString(utf8);
  }

  AString resultString;
  for (int i = 0; i < srcString.Len(); i++)
  {
    if (srcString[i] >= 256) resultString += '?';
    else                     resultString += char(srcString[i]);
  }
  return resultString;
}

#else /* __APPLE_CC__ */

#include "UTFConvert.h"

UString MultiByteToUnicodeString(const AString &srcString, UINT codePage)
{
  if ((global_use_utf16_conversion) && (!srcString.IsEmpty()))
  {
    UString resultString;
    bool bret = ConvertUTF8ToUnicode(srcString,resultString);
    if (bret) return resultString;
  }

  UString resultString;
  for (int i = 0; i < srcString.Len(); i++)
    resultString += wchar_t(srcString[i] & 255);

  return resultString;
}

AString UnicodeStringToMultiByte(const UString &srcString, UINT codePage)
{
  if ((global_use_utf16_conversion) && (!srcString.IsEmpty()))
  {
    AString resultString;
    ConvertUnicodeToUTF8(srcString,resultString);
    return resultString;
  }

  AString resultString;
  for (int i = 0; i < srcString.Len(); i++)
  {
    if (srcString[i] >= 256) resultString += '?';
    else                     resultString += char(srcString[i]);
  }
  return resultString;
}

#endif /* __APPLE_CC__ */

#else /* LOCALE_IS_UTF8 */

UString MultiByteToUnicodeString(const AString &srcString, UINT /* codePage */ )
{
#ifdef ENV_HAVE_MBSTOWCS
  if ((global_use_utf16_conversion) && (!srcString.IsEmpty()))
  {
    UString resultString;
    int numChars = mbstowcs(resultString.GetBuf(srcString.Len()),srcString,srcString.Len()+1);
    if (numChars >= 0) {
        resultString.ReleaseBuf_SetEnd(numChars);

#if WCHAR_MAX > 0xffff
      for (int i = numChars; i >= 0; i--) {
        if (resultString[i] > 0xffff) {
          wchar_t c = resultString[i] - 0x10000;
          resultString.Delete(i);
          wchar_t texts[]= { ((c >> 10) & 0x3ff) + 0xd800,  (c & 0x3ff) + 0xdc00 , 0 };
          resultString.Insert(i, texts);
          numChars++;
        }
      }
#endif

      return resultString;
    }
  }
#endif

  UString resultString;
  for (int i = 0; i < srcString.Len(); i++)
    resultString += wchar_t(srcString[i] & 255);

  return resultString;
}

AString UnicodeStringToMultiByte(const UString &src, UINT /* codePage */ )
{
#ifdef ENV_HAVE_WCSTOMBS
#if WCHAR_MAX > 0xffff
  UString srcString(src);
  for (int i = 0; i < srcString.Len(); i++) {
    if ((0xd800 <= srcString[i] && srcString[i] <= 0xdbff) && ((i + 1) < srcString.Len()) &&
        (0xdc00 <= srcString[i + 1] && srcString[i + 1] < 0xE000)) {
      wchar_t c = (((srcString[i] - 0xd800) << 10) | (srcString[i + 1] - 0xdc00)) + 0x10000;
      srcString.Delete(i, 2);
      srcString.Insert(i, c);
    }
  }
#else
  const UString &srcString = src;
#endif

  if ((global_use_utf16_conversion) && (!srcString.IsEmpty()))
  {
    AString resultString;
    int numRequiredBytes = srcString.Len() * 6+1;
    int numChars = wcstombs(resultString.GetBuf(numRequiredBytes),srcString,numRequiredBytes);
    if (numChars >= 0) {
      resultString.ReleaseBuf_SetEnd(numChars);
      return resultString;
    }
  }
#else
  const UString &srcString = src;
#endif

  AString resultString;
  for (int i = 0; i < srcString.Len(); i++)
  {
    if (srcString[i] >= 256) resultString += '?';
    else                     resultString += char(srcString[i]);
  }
  return resultString;
}

#endif /* LOCALE_IS_UTF8 */

void MultiByteToUnicodeString2(UString &dest, const AString &srcString, UINT codePage)
{
  dest = MultiByteToUnicodeString(srcString,codePage);
}

void UnicodeStringToMultiByte2(AString &dest, const UString &srcString, UINT codePage)
{
  dest = UnicodeStringToMultiByte(srcString,codePage);
}

UString MultiByteToUnicodeString3(const AString &srcString, iconv_t convBase)
{
    if (srcString.IsEmpty() || convBase == 0 || convBase == (iconv_t)-1)
    {
        return MultiByteToUnicodeString(srcString, 0);
    }

    UString resultString;
    CFStringRef cfpath;
    const char * path = &srcString[0];

    char out[1024];
    char *outPtr = out;
    size_t outSize = 1024;
    char *inPtr = (char *)&srcString[0];
    size_t inSize = strlen(path);

    while (iconv(convBase, &inPtr, &inSize, &outPtr, &outSize) == (size_t)-1)
    {
      if (!outSize || !inSize) break;
      *outPtr = '?';
      outPtr++;
      outSize--;
      inPtr++;
      inSize--;
      if (!outSize || !inSize) break;
    }
    *outPtr = 0;

    return MultiByteToUnicodeString(out, 0);
}

AString UnicodeStringToMultiByte3(const UString &srcString, iconv_t convBase)
{
    AString utf8 = UnicodeStringToMultiByte(srcString, 0);
    if (convBase == 0 || convBase == (iconv_t)-1) {
        return utf8;
    }

    char out[1024];
    char *outPtr = out;
    size_t outSize = 1024;
    char *inPtr = (char *)&utf8[0];;
    size_t inSize = strlen(utf8);

    while (iconv(convBase, &inPtr, &inSize, &outPtr, &outSize) == (size_t)-1)
    {
      if (!outSize || !inSize) break;
      *outPtr = '?';
      outPtr++;
      outSize--;
      int chw = 1;
      unsigned char cp = (unsigned char)(*inPtr);
      if (cp < 0x80)
      {
        chw = 1;
      }
      else if (cp < 0xe0)
      {
        chw = 2;
      }
      else if (cp < 0xf0)
      {
        chw = 3;
      }
      else
      {
        chw = 4;
      }
      inPtr += chw;
      inSize -= chw;
      if (!outSize || !inSize) break;
    }
    *outPtr = 0;
    return AString(out);
}
<|endoftext|>


<commit_before>/*-
 * Copyright 2017-2018 Shivansh Rai
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. 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.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR 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 AUTHOR 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.
 *
 * $FreeBSD$
 */

#include <dirent.h>
#include <sys/stat.h>
#include <unistd.h>

#include <boost/filesystem.hpp>
#include <boost/thread.hpp>
#include <fstream>
#include <iostream>
#include <regex>

#include "fetch_groff.h"
#include "logging.h"

/* Map of utility name and its location in src tree. */
std::unordered_map<std::string, std::string> groff::groff_map;

/*
 * Traverses the FreeBSD src tree looking for groff scripts for section 1 and
 * section 8 utilities and stores their location in a hashmap.
 */
int
groff::FetchGroffScripts()
{
	std::string utils_list = "scripts/utils_list";
	std::string src = "../../../";  /* FreeBSD src. */
	std::string utildir;
	std::string utilname;
	std::string path;
	std::ifstream file;
	std::regex section ("(.*).(?:1|8)");
	struct stat sb;
	struct dirent *ent;
	DIR *dir;

	/* Check if the file "scripts/utils_list" exists. */
	if (stat(utils_list.c_str(), &sb) != 0) {
		std::cerr << "scripts/utils_list does not exists.\n"
			     "Run 'make fetch_utils' first.\n";
		return EXIT_FAILURE;
	}
	file.open(utils_list);

	while (getline(file, utildir)) {
		path = src + utildir + "/tests";
		/*
		 * Copy the groff script only if the utility does not already
		 * have tests, i.e. the "tests" directory is absent.
		 */
		if (stat(path.c_str(), &sb) || !S_ISDIR(sb.st_mode)) {
			path = src + utildir + "/";
			utilname = utildir.substr(utildir.find_last_of("/") + 1);
			if ((dir = opendir(path.c_str())) != NULL) {
				/* Skip directory entry for "." and "..". */
				ent = readdir(dir);
				ent = readdir(dir);
				while ((ent = readdir(dir)) != NULL) {
					if (std::regex_match(ent->d_name, section)) {
						groff_map[utilname] = path + ent->d_name;
					}
				}
				closedir(dir);
			} else {
				logging::LogPerror("opendir()");
			}
		}
	}

	file.close();
	return EXIT_SUCCESS;
}
<commit_msg>Add explicit check for verifying directory entries for '.' and '..'<commit_after>/*-
 * Copyright 2017-2018 Shivansh Rai
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. 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.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR 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 AUTHOR 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.
 *
 * $FreeBSD$
 */

#include <dirent.h>
#include <sys/stat.h>
#include <unistd.h>

#include <boost/filesystem.hpp>
#include <boost/thread.hpp>
#include <fstream>
#include <iostream>
#include <regex>

#include "fetch_groff.h"
#include "logging.h"

/* Map of utility name and its location in src tree. */
std::unordered_map<std::string, std::string> groff::groff_map;

/*
 * Traverses the FreeBSD src tree looking for groff scripts for section 1 and
 * section 8 utilities and stores their location in a hashmap.
 */
int
groff::FetchGroffScripts()
{
	std::string utils_list = "scripts/utils_list";
	std::string src = "../../../";  /* FreeBSD src. */
	std::string utildir;
	std::string utilname;
	std::string path;
	std::ifstream file;
	std::regex section ("(.*).(?:1|8)");
	struct stat sb;
	struct dirent *ent;
	DIR *dir;

	/* Check if the file "scripts/utils_list" exists. */
	if (stat(utils_list.c_str(), &sb) != 0) {
		std::cerr << "scripts/utils_list does not exists.\n"
			     "Run 'make fetch_utils' first.\n";
		return EXIT_FAILURE;
	}
	file.open(utils_list);

	while (getline(file, utildir)) {
		path = src + utildir + "/tests";
		/*
		 * Copy the groff script only if the utility does not already
		 * have tests, i.e. the "tests" directory is absent.
		 */
		if (stat(path.c_str(), &sb) || !S_ISDIR(sb.st_mode)) {
			path = src + utildir + "/";
			utilname = utildir.substr(utildir.find_last_of("/") + 1);
			if ((dir = opendir(path.c_str())) != NULL) {
				while ((ent = readdir(dir)) != NULL) {
					if (!strcmp(ent->d_name, ".") || !strcmp(ent->d_name, "..")) {
						continue;
					} else if (std::regex_match(ent->d_name, section)) {
						groff_map[utilname] = path + ent->d_name;
					}
				}
				closedir(dir);
			} else {
				logging::LogPerror("opendir()");
			}
		}
	}

	file.close();
	return EXIT_SUCCESS;
}
<|endoftext|>