void ground_map_loop()
{
// check for new position
int pos = (int)( get_lat_position_cm() / MAP_RESOLUTION );
if( pos > last_pos ) {
//map_sensor_to_index( RIGHT_DISTANCE_SENSOR, mod_pos( pos + MAP_SIZE/2, MAP_SIZE ) );
ground_map_buffer[mod_pos( pos + MAP_SIZE/2, MAP_SIZE )] = get_distance( RIGHT_DISTANCE_SENSOR );
} else if( pos < last_pos ) {
ground_map_buffer[mod_pos( pos - MAP_SIZE/2, MAP_SIZE )] = get_distance( LEFT_DISTANCE_SENSOR );
//map_sensor_to_index( LEFT_DISTANCE_SENSOR, mod_pos( pos - MAP_SIZE/2, MAP_SIZE ) );
}
// check if in safe zone
static bool cur_safe = false;
if( last_pos != pos ) {
bool new_safe = safe_zone();
if( new_safe != cur_safe ) {
cur_safe = new_safe;
if( new_safe ) {
Serial.println( "Safe" );
digitalWrite( 13, HIGH );
set_lat_action( MOTOR_STOP );
} else {
Serial.println( "Not safe" );
digitalWrite( 13, LOW );
}
}
}
last_pos = pos;
}
bool safe_zone()
{
unsigned start_pos = mod_pos( last_pos - SEAT_SIZE/2, MAP_SIZE );
float min_height = ground_map_buffer[start_pos];
float max_height = ground_map_buffer[start_pos];
// find MIN and MAX for the seat area
for( int i = 1; i < SEAT_SIZE; i++ ) {
unsigned pos_i = (start_pos + i)%MAP_SIZE;
if( ground_map_buffer[pos_i] < min_height ) {
min_height = ground_map_buffer[pos_i];
} else if( ground_map_buffer[pos_i] < max_height ) {
max_height = ground_map_buffer[pos_i];
}
}
float ave_height = (max_height + min_height) / 2.0;
if( ((max_height - min_height) < SAFE_SURFACE_NOISE) &&
(ave_height < SAFE_MAX_HEIGHT) &&
(ave_height > SAFE_MIN_HEIGHT) )
{
return true;
}
return false;
}
void print_ground_map()
{
Serial.print( "I: " );
Serial.println( last_pos );
for( int i = 0; i < MAP_SIZE; i++ ) {
Serial.print( " " );
Serial.println( ground_map_buffer[mod_pos( i + last_pos - MAP_SIZE/2, MAP_SIZE )] );
}
}
MergedCode SequenceSpace::getMergedCode( const ModificationSequence& mod_seq )
{
MergedCode merged_code;
std::string str;
Branch& bc = gs->getBranchByID(0);
ModificationSequence::const_iterator iter = mod_seq.find("SO3");
const ModificationSites& s_sites = iter->second;
for(size_t i = 0; i < bc.getUnitNum(); i++)
{
std::string symbol = bc.getUnitByID(i).getSymbol();
if(symbol == "DeltaGlcA") {
str = "D";
} else if(symbol == "GlcA") {
str = "G";
} else if(symbol == "GlcN") { // A, S or H.
// Check the 2-N position.
ModificationPosition mod_pos(0, i, 2);
ModificationSequence::const_iterator ac_iter = mod_seq.find("Ac");
ModificationSites::const_iterator s_pos_iter = s_sites.find(mod_pos);
if(ac_iter != mod_seq.end()) {
const ModificationSites& ac_sites = ac_iter->second;
ModificationSites::const_iterator ac_pos_iter = ac_sites.find(mod_pos);
if(ac_pos_iter != ac_sites.end())
str = "A";
/*else if(s_pos_iter != s_sites.end())
str = "S";*/
else {
str = "H";
}
} else {
//if(s_pos_iter != s_sites.end())
// str = "S";
//else {
// str = "H";
//}
str = "H";
}
} else if(symbol == "GlcNAc") {
str = "A";
} else {
str = "X"; // Unknown type.
}
if(iter == mod_seq.end()) {
merged_code.push_back(std::make_pair(str,0));
continue;
}
size_t num = 0;
// Get the number of SO3 on the ring.
if(str == "D" || str == "G") {
ModificationSites::const_iterator pos_iter = s_sites.find(ModificationPosition(0, i, 2));
if(pos_iter != s_sites.end()) {
num = 1;
}
} else if(str == "H" || str == "A") {
for(size_t j = 1; j < bc.getUnitByID(i).getInternalSites().size(); j++)
{
ModificationSites::const_iterator pos_iter = s_sites.find(ModificationPosition(0, i, j));
if(pos_iter != s_sites.end())
num += 1;
}
} else {
// Do nothing.
}
merged_code.push_back(std::make_pair(str,num));
}
return merged_code;
}
