// Sets the text on the control
//--------------------------------------------------------------------------------
void CPUTButton::SetText(const std::string String)
{
// Zero out the size and location
InitializeState();
// create the static text object if it doesn't exist
if(NULL == mpButtonText)
{
mpButtonText = CPUTText::Create(mpFont);
}
// set the Static control's text
mpButtonText->SetText(String);
// get the dimensions of the string in pixels
CPUT_RECT rect;
mpButtonText->GetDimensions(rect.width, rect.height);
// resize this control to fix that string with padding
Resize(rect.width, rect.height);
// move the text to a nice inset location inside the 'safe' area
// of the button image
int x,y;
GetInsetTextCoordinate(x, y);
mpButtonText->SetPosition(x, y);
}
void DPTreephaser::QueryState(BasecallerRead& data, vector<float>& query_state, int& current_hp, int max_flows, int query_flow)
{
max_flows = min(max_flows,flow_order_.num_flows());
assert(query_flow < max_flows);
InitializeState(&path_[0]);
query_state.assign(max_flows,0);
char myNuc = 'N';
for (vector<char>::iterator nuc = data.sequence.begin(); nuc != data.sequence.end() and path_[0].flow <= query_flow; ++nuc) {
if (path_[0].flow == query_flow and myNuc != 'N' and myNuc != *nuc)
break;
AdvanceStateInPlace(&path_[0], *nuc, flow_order_.num_flows());
if (path_[0].flow == query_flow and myNuc == 'N')
myNuc = *nuc;
}
// Catching cases where a query_flow without incorporation or query_flow after end of sequence was given
int until_flow = min(path_[0].window_end, max_flows);
if (path_[0].flow == query_flow) {
current_hp = path_[0].last_hp;
for (int flow = path_[0].window_start; flow < until_flow; ++flow)
query_state[flow] = path_[0].state[flow];
}
else
current_hp = 0;
}
void DPTreephaser::QueryAllStates(BasecallerRead& data, vector< vector<float> >& query_states, vector<int>& hp_lengths, int max_flows)
{
max_flows = min(max_flows,flow_order_.num_flows());
InitializeState(&path_[0]);
max_flows = min(max_flows, flow_order_.num_flows());
query_states.reserve(data.sequence.size());
query_states.resize(0);
hp_lengths.assign(data.sequence.size(), 0);
char last_nuc = 'N';
int hp_count = 0;
for (vector<char>::iterator nuc = data.sequence.begin(); nuc != data.sequence.end() and path_[0].flow < max_flows; ++nuc) {
if (last_nuc != *nuc and last_nuc != 'N') {
hp_lengths[hp_count] = path_[0].last_hp;
query_states.push_back(path_[0].state);
hp_count++;
}
AdvanceStateInPlace(&path_[0], *nuc, max_flows);
last_nuc = *nuc;
}
hp_lengths[hp_count] = path_[0].last_hp;
query_states.push_back(path_[0].state);
hp_lengths.resize(query_states.size());
data.prediction.swap(path_[0].prediction);
}
void ESVideo::Initialize ()
{
ESVideoPlatform::Initialize(ScreenWidth, ScreenHeight);
UIShader = new LibESGL::Program(vert, frag, false, false);
UIShader->Use();
#ifdef GLSL_IS_MY_FRIEND
glUniform1f(UIShader->ObtainToken("screenWidth"), ScreenWidth);
glUniform1f(UIShader->ObtainToken("screenHeight"), ScreenHeight);
glUniform1i(UIShader->ObtainToken("tex"), 0);
#else
cgGLSetParameter1f((CGparameter)UIShader->ObtainToken("screenWidth"), ScreenWidth);
cgGLSetParameter1f((CGparameter)UIShader->ObtainToken("screenHeight"), ScreenHeight);
#endif
//Some settings
InitializeState();
// Setup vertex buffer
VertexBuffer = (GLfloat*)malloc(VertexBufferCount * VertexSize * sizeof(GLfloat));
ApplyVertexBuffer(VertexBuffer);
//Texture for FillRectangle
FillerTexture = new Texture(2, 2);
FillerTexture->Clear(0xFFFFFFFF);
//Init framebuffer
glGenFramebuffersEXT(1, &FrameBufferID); glSplat();
}
// Constructor
//-----------------------------------------------------------------------------
CPUTButton::CPUTButton(const cString ControlText, CPUTControlID id, CPUTFont *pFont):
mbStartedClickInside(false),
mpButtonText(NULL),
mpMirrorBufferActive(NULL),
mpMirrorBufferPressed(NULL),
mpMirrorBufferDisabled(NULL),
mpFont(pFont)
{
// initialize the state variables
InitializeState();
// save the control ID for callbacks
mcontrolID = id;
// save the font to use for text on this button
mpFont = pFont;
// set as enabled
CPUTControl::SetEnable(true);
// initialize the size lists
memset(&mpButtonIdleSizeList, 0, sizeof(CPUT_SIZE) * CPUT_NUM_IMAGES_IN_BUTTON);
memset(&mpButtonPressedSizeList, 0, sizeof(CPUT_SIZE) * CPUT_NUM_IMAGES_IN_BUTTON);
memset(&mpButtonDisabledSizeList, 0, sizeof(CPUT_SIZE) * CPUT_NUM_IMAGES_IN_BUTTON);
// set up the per-instance data
RegisterInstanceResources();
// set the text on the button and resize it accordingly
SetText(ControlText);
// set the default control position
SetPosition( 0, 0 );
}
void InitializeVGAScreenMode175(uint8_t *framebuffer,int pixelsperrow,int pixelclock)
{
HBlankInterruptHandler=NULL;
InitializeVGAPort();
InitializeState(framebuffer,pixelsperrow);
InitializePixelDMA(pixelclock);
InitializeVGAHorizontalSync31kHz(HSYNCHandler175);
}
void ESVideo::SetScreenSize (uint32_t aX, uint32_t aY)
{
ScreenWidth = aX;
ScreenHeight = aY;
InitializeState();
FontManager::InitFonts();
}
void DPTreephaser::Simulate(BasecallerRead& data, int max_flows)
{
InitializeState(&path_[0]);
for (vector<char>::iterator nuc = data.sequence.begin(); nuc != data.sequence.end() and path_[0].flow < max_flows; ++nuc)
AdvanceStateInPlace(&path_[0], *nuc, flow_order_.num_flows());
data.prediction.swap(path_[0].prediction);
}
void DPTreephaser::Simulate(BasecallerRead& data, int max_flows)
{
max_flows = min(max_flows,flow_order_.num_flows());
InitializeState(&path_[0]);
for (int solution_flow = 0; solution_flow < max_flows; ++solution_flow)
for (int hp = 0; hp < data.solution[solution_flow]; ++hp)
AdvanceStateInPlace(&path_[0], flow_order_.int_at(solution_flow), flow_order_.num_flows());
data.prediction.swap(path_[0].prediction);
}
void DPTreephaser::Simulate(BasecallerRead& data, int max_flows,bool state_inphase)
{
InitializeState(&path_[0]);
for (vector<char>::iterator nuc = data.sequence.begin(); nuc != data.sequence.end()
and path_[0].flow < max_flows; ++nuc) {
AdvanceStateInPlace(&path_[0], *nuc, flow_order_.num_flows());
path_[0].sequence.push_back(*nuc); // Needed to simulate diagonal states correctly
if (state_inphase and path_[0].flow < max_flows)
data.state_inphase.at(path_[0].flow) = path_[0].state.at(path_[0].flow);
}
data.prediction.swap(path_[0].prediction);
}
OpenGLFrameBuffer::OpenGLFrameBuffer(void *hMonitor, int width, int height, int bits, int refreshHz, bool fullscreen) :
Super(hMonitor, width, height, bits, refreshHz, fullscreen)
{
GLRenderer = new FGLRenderer(this);
memcpy (SourcePalette, GPalette.BaseColors, sizeof(PalEntry)*256);
UpdatePalette ();
ScreenshotBuffer = NULL;
LastCamera = NULL;
InitializeState();
gl_SetupMenu();
gl_GenerateGlobalBrightmapFromColormap();
DoSetGamma();
needsetgamma = true;
swapped = false;
Accel2D = true;
SetVSync(vid_vsync);
}
void Tracker::measurementCallBack(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
{
measurement.header = msg->header;
measurement.pose = msg->pose;
updatedStateToPublish.header = msg->header;
if(previousMeasurementMessageTime == currentMeasurementMessageTime)// This is the first time measurement is obtained
{
currentMeasurementMessageTime = ros::Time(msg->header.stamp.sec, msg->header.stamp.nsec);
//Only initialize the state
InitializeState();
return;
}
else
{
previousMeasurementMessageTime = currentMeasurementMessageTime;
currentMeasurementMessageTime = ros::Time(msg->header.stamp.sec, msg->header.stamp.nsec);
ros::Duration timediff = currentMeasurementMessageTime - previousMeasurementMessageTime;
deltaT = timediff.toSec();
//just a safety feature
if(deltaT>0)
{
InitializeJacobianG(); // because we have a new deltaT
ConstructNoiseMatrixQ();
predict();
update();
}
}
}
// Sets the text on the control
//--------------------------------------------------------------------------------
void CPUTButton::SetText(const cString String)
{
// Zero out the size and location
InitializeState();
// create the static text object if it doesn't exist
if(NULL == mpButtonText)
{
mpButtonText = new CPUTText(mpFont);
}
// set the Static control's text
mpButtonText->SetText(String);
// get the dimensions of the string in pixels
CPUT_RECT rect;
mpButtonText->GetDimensions(rect.width, rect.height);
// resize this control to fix that string with padding
Resize(rect.width, rect.height);
// move the text to a nice inset location inside the 'safe' area
// of the button image
int x,y;
GetInsetTextCoordinate(x, y);
mpButtonText->SetPosition(x, y);
// position or size may move - force a recalculation of this control's location
// if it is managed by the auto-arrange function
if(this->IsAutoArranged())
{
mControlNeedsArrangmentResizing = true;
}
else
{
// otherwise, we mark this as dirty
mControlGraphicsDirty = true;
}
}
// sets the dimensions of the button
//--------------------------------------------------------------------------------
void CPUTButton::SetDimensions(int width, int height)
{
// Zero out the size and location
InitializeState();
if(mControlAutoArranged)
{
// get the dimensions of the string in pixels
CPUT_RECT rect;
mpButtonText->GetDimensions(rect.width, rect.height);
width = std::max(rect.width, width);
height = std::max(rect.height, height);
}
// resize this control to fix that string with padding
Resize(width, height);
// move the text to a nice inset location inside the 'safe' area
// of the button image
int x,y;
GetInsetTextCoordinate(x, y);
mpButtonText->SetPosition(x, y);
}
void DPTreephaser::SimulateRecalibrated(BasecallerRead& data, int max_flows)
{
InitializeState(&path_[0]);
// Generate predicted signal
for (vector<char>::iterator nuc = data.sequence.begin(); nuc != data.sequence.end() and path_[0].flow < max_flows; ++nuc) {
int flow_s = path_[0].flow;
AdvanceStateInPlace(&path_[0], *nuc, flow_order_.num_flows());
if (path_[0].flow < flow_order_.num_flows()) {
//update flow2base_pos
path_[0].base_pos++;
path_[0].flow2base_pos[path_[0].flow] = path_[0].base_pos;
for(int flow_inter = flow_s+1; flow_inter < path_[0].flow; flow_inter++)
path_[0].flow2base_pos[flow_inter] = path_[0].flow2base_pos[flow_s];
}
}
// Apply signal distortion according to HP recalibration model
if (pm_model_available_) {
for (int flow = 0; flow < flow_order_.num_flows(); ++flow) {
int hp_length = 0;
if(flow == 0)
hp_length = path_[0].flow2base_pos[0];
else
hp_length =path_[0].flow2base_pos[flow] - path_[0].flow2base_pos[flow-1];
if(hp_length < 0)
hp_length = 0;
if(hp_length > MAX_HPXLEN)
hp_length = MAX_HPXLEN;
path_[0].prediction[flow] = path_[0].prediction[flow] * (*As_)[flow][flow_order_.int_at(flow)][hp_length]
+ (*Bs_)[flow][flow_order_.int_at(flow)][hp_length];
}
}
data.prediction.swap(path_[0].prediction);
}
// ------------------------------------------------------------------------
// Compute quality metrics
int DPTreephaser::ComputeQVmetrics(BasecallerRead& read)
{
read.state_inphase.assign(flow_order_.num_flows(), 1);
read.state_total.assign(flow_order_.num_flows(), 1);
int num_bases = 0;
for (int flow = 0; flow < flow_order_.num_flows(); ++flow)
num_bases += read.solution[flow];
if (num_bases == 0)
return 0;
read.penalty_mismatch.assign(num_bases, 0);
read.penalty_residual.assign(num_bases, 0);
int max_flows = flow_order_.num_flows();
TreephaserPath *parent = &path_[0];
TreephaserPath *children[4] = { &path_[1], &path_[2], &path_[3], &path_[4] };
InitializeState(parent);
int base = 0;
float recent_state_inphase = 1;
float recent_state_total = 1;
// main loop for base calling
for (int solution_flow = 0; solution_flow < max_flows; ++solution_flow) {
for (int hp = 0; hp < read.solution[solution_flow]; ++hp) {
float penalty[4] = { 0, 0, 0, 0 };
for (int nuc = 0; nuc < 4; nuc++) {
TreephaserPath *child = children[nuc];
AdvanceState(child, parent, nuc, max_flows);
// Apply easy termination rules
if (child->flow >= max_flows) {
penalty[nuc] = 25; // Mark for deletion
continue;
}
if (parent->solution[child->flow] >= kMaxHP) {
penalty[nuc] = 25; // Mark for deletion
continue;
}
for (int flow = parent->window_start; flow < child->window_end; ++flow) {
float residual = read.normalized_measurements[flow] - child->prediction[flow];
if (residual <= 0 or flow < child->flow)
penalty[nuc] += residual*residual;
}
} //looping over nucs
// find current incorporating base
int called_nuc = flow_order_.int_at(solution_flow);
assert(children[called_nuc]->flow == solution_flow);
recent_state_inphase = children[called_nuc]->state[solution_flow];
recent_state_total = 0;
for (int flow = children[called_nuc]->window_start; flow < children[called_nuc]->window_end; ++flow)
recent_state_total += children[called_nuc]->state[flow];
// Get delta penalty to next best solution
read.penalty_mismatch[base] = -1; // min delta penalty to earlier base hypothesis
read.penalty_residual[base] = 0;
if (solution_flow - parent->window_start > 0)
read.penalty_residual[base] = penalty[called_nuc] / (solution_flow - parent->window_start);
for (int nuc = 0; nuc < 4; ++nuc) {
if (nuc == called_nuc)
continue;
float penalty_mismatch = penalty[called_nuc] - penalty[nuc];
read.penalty_mismatch[base] = max(read.penalty_mismatch[base], penalty_mismatch);
}
// Fill out the remaining portion of the prediction
for (int flow = 0; flow < parent->window_start; ++flow)
children[called_nuc]->prediction[flow] = parent->prediction[flow];
for (int flow = children[called_nuc]->window_end; flow < max_flows; ++flow)
children[called_nuc]->prediction[flow] = 0;
// Called state is the starting point for next base
TreephaserPath *swap = parent;
parent = children[called_nuc];
children[called_nuc] = swap;
base++;
}
read.state_inphase[solution_flow] = max(recent_state_inphase, 0.01f);
read.state_total[solution_flow] = max(recent_state_total, 0.01f);
}
return num_bases;
}
void DPTreephaser::Solve(BasecallerRead& read, int max_flows, int restart_flows)
{
static const char nuc_int_to_char[5] = "ACGT";
assert(max_flows <= flow_order_.num_flows());
// Initialize stack: just one root path
for (int p = 1; p < kNumPaths; ++p)
path_[p].in_use = false;
InitializeState(&path_[0]);
path_[0].path_metric = 0;
path_[0].per_flow_metric = 0;
path_[0].residual_left_of_window = 0;
path_[0].dot_counter = 0;
path_[0].in_use = true;
//path_[0].sequence.reserve(2*flow_order_.num_flows()); //Done in InitializeState
int space_on_stack = kNumPaths - 1;
float sum_of_squares_upper_bound = 1e20; //max_flows; // Squared distance of solution to measurements
if (restart_flows > 0) {
// The solver will not attempt to solve initial restart_flows
// - Simulate restart_flows instead of solving
// - If it turns out that solving was finished before restart_flows, simply exit without any changes to the read.
restart_flows = min(restart_flows, flow_order_.num_flows());
for (vector<char>::iterator nuc = read.sequence.begin(); nuc != read.sequence.end() and path_[0].flow < restart_flows; ++nuc) {
int flow_s = path_[0].flow;
AdvanceStateInPlace(&path_[0], *nuc, flow_order_.num_flows());
if (path_[0].flow < flow_order_.num_flows()) {
path_[0].sequence.push_back(*nuc);
//update flow2base_pos
path_[0].base_pos++;
path_[0].flow2base_pos[path_[0].flow] = path_[0].base_pos;
for(int flow_inter = flow_s+1; flow_inter < path_[0].flow; flow_inter++)
path_[0].flow2base_pos[flow_inter] = path_[0].flow2base_pos[flow_s];
}
}
if (path_[0].flow < restart_flows-10) { // This read ended before restart_flows. No point resolving it.
read.prediction.swap(path_[0].prediction);
return;
}
for (int flow = 0; flow < path_[0].window_start; ++flow) {
float residual = 0;
if(pm_model_enabled_==false){
residual = read.normalized_measurements[flow] - path_[0].prediction[flow];
}
else{
int hp_length = 0;
if(flow==0) hp_length = path_[0].flow2base_pos[0];
else hp_length =path_[0].flow2base_pos[flow] - path_[0].flow2base_pos[flow-1];
if(hp_length<0) hp_length = 0;
residual = read.normalized_measurements[flow]
- (path_[0].prediction[flow] * (*As_)[flow][flow_order_.int_at(flow)][hp_length]
+ (*Bs_)[flow][flow_order_.int_at(flow)][hp_length]);
}
path_[0].residual_left_of_window += residual * residual;
}
}
// Initializing variables
//read.solution.assign(flow_order_.num_flows(), 0);
read.sequence.clear();
read.sequence.reserve(2*flow_order_.num_flows());
read.prediction.assign(flow_order_.num_flows(), 0);
// Main loop to select / expand / delete paths
while (1) {
// ------------------------------------------
// Step 1: Prune the content of the stack and make sure there are at least 4 empty slots
// Remove paths that are more than 'maxPathDelay' behind the longest one
if (space_on_stack < kNumPaths-3) {
int longest_path = 0;
for (int p = 0; p < kNumPaths; ++p)
if (path_[p].in_use)
longest_path = max(longest_path, path_[p].flow);
if (longest_path > kMaxPathDelay) {
for (int p = 0; p < kNumPaths; ++p) {
if (path_[p].in_use and path_[p].flow < longest_path-kMaxPathDelay) {
path_[p].in_use = false;
space_on_stack++;
}
}
}
}
// If necessary, remove paths with worst perFlowMetric
while (space_on_stack < 4) {
// find maximum per flow metric
float max_per_flow_metric = -0.1;
int max_metric_path = kNumPaths;
for (int p = 0; p < kNumPaths; ++p) {
if (path_[p].in_use and path_[p].per_flow_metric > max_per_flow_metric) {
max_per_flow_metric = path_[p].per_flow_metric;
max_metric_path = p;
}
}
// killing path with largest per flow metric
if (!(max_metric_path < kNumPaths)) {
printf("Failed assertion in Treephaser\n");
for (int p = 0; p < kNumPaths; ++p) {
if (path_[p].in_use)
printf("Path %d, in_use = true, per_flow_metric = %f\n", p, path_[p].per_flow_metric);
else
printf("Path %d, in_use = false, per_flow_metric = %f\n", p, path_[p].per_flow_metric);
}
fflush(NULL);
}
assert (max_metric_path < kNumPaths);
path_[max_metric_path].in_use = false;
space_on_stack++;
}
// ------------------------------------------
// Step 2: Select a path to expand or break if there is none
TreephaserPath *parent = NULL;
float min_path_metric = 1000;
for (int p = 0; p < kNumPaths; ++p) {
if (path_[p].in_use and path_[p].path_metric < min_path_metric) {
min_path_metric = path_[p].path_metric;
parent = &path_[p];
}
}
if (!parent)
break;
// ------------------------------------------
// Step 3: Construct four expanded paths and calculate feasibility metrics
assert (space_on_stack >= 4);
TreephaserPath *children[4];
for (int nuc = 0, p = 0; nuc < 4; ++p)
if (not path_[p].in_use)
children[nuc++] = &path_[p];
float penalty[4] = { 0, 0, 0, 0 };
for (int nuc = 0; nuc < 4; ++nuc) {
TreephaserPath *child = children[nuc];
AdvanceState(child, parent, nuc_int_to_char[nuc], max_flows);
// Apply easy termination rules
if (child->flow >= max_flows) {
penalty[nuc] = 25; // Mark for deletion
continue;
}
if (child->last_hp > kMaxHP) {
penalty[nuc] = 25; // Mark for deletion
continue;
}
if ((int)parent->sequence.size() >= (2 * flow_order_.num_flows() - 10)) {
penalty[nuc] = 25; // Mark for deletion
continue;
}
child->path_metric = parent->residual_left_of_window;
child->residual_left_of_window = parent->residual_left_of_window;
float penaltyN = 0;
float penalty1 = 0;
for (int flow = parent->window_start; flow < child->window_end; ++flow) {
float residual = 0;
if(pm_model_enabled_==false){
residual = read.normalized_measurements[flow] - child->prediction[flow];
}
else{
int hp_length = 0;
if(flow==0)
hp_length = parent->flow2base_pos[0];
else
hp_length = parent->flow2base_pos[flow] - parent->flow2base_pos[flow-1];
if(hp_length<0) hp_length = 0;
residual = read.normalized_measurements[flow]
- (child->prediction[flow] * (*As_)[flow][flow_order_.int_at(flow)][hp_length]
+ (*Bs_)[flow][flow_order_.int_at(flow)][hp_length]);
}
float residual_squared = residual * residual;
// Metric calculation
if (flow < child->window_start) {
child->residual_left_of_window += residual_squared;
child->path_metric += residual_squared;
} else if (residual <= 0)
child->path_metric += residual_squared;
if (residual <= 0)
penaltyN += residual_squared;
else if (flow < child->flow)
penalty1 += residual_squared;
}
penalty[nuc] = penalty1 + kNegativeMultiplier * penaltyN;
penalty1 += penaltyN;
if (child->flow>0)
child->per_flow_metric = (child->path_metric + 0.5 * penalty1) / child->flow;
} //looping over nucs
// Find out which nuc has the least penalty (the greedy choice nuc)
int best_nuc = 0;
if (penalty[best_nuc] > penalty[1])
best_nuc = 1;
if (penalty[best_nuc] > penalty[2])
best_nuc = 2;
if (penalty[best_nuc] > penalty[3])
best_nuc = 3;
// ------------------------------------------
// Step 4: Use calculated metrics to decide which paths are worth keeping
for (int nuc = 0; nuc < 4; ++nuc) {
TreephaserPath *child = children[nuc];
// Path termination rules
if (penalty[nuc] >= 20)
continue;
if (child->path_metric > sum_of_squares_upper_bound)
continue;
// This is the only rule that depends on finding the "best nuc"
if (penalty[nuc] - penalty[best_nuc] >= kExtendThreshold)
continue;
float dot_signal = 0;
if(pm_model_enabled_==false){
dot_signal = (read.normalized_measurements[child->flow] - parent->prediction[child->flow]) / child->state[child->flow];
}
else{
int hp_length = 0;
if(child->flow==0) hp_length = parent->flow2base_pos[0];
else hp_length = parent->flow2base_pos[child->flow] - parent->flow2base_pos[child->flow-1];
if(hp_length<0) hp_length = 0;
dot_signal = (read.normalized_measurements[child->flow] - (parent->prediction[child->flow]
* (*As_)[child->flow][flow_order_.int_at(child->flow)][hp_length]
+ (*Bs_)[child->flow][flow_order_.int_at(child->flow)][hp_length]))
/ child->state[child->flow];
}
child->dot_counter = (dot_signal < kDotThreshold) ? (parent->dot_counter + 1) : 0;
if (child->dot_counter > 1)
continue;
// Path survived termination rules and will be kept on stack
child->in_use = true;
space_on_stack--;
// Fill out the remaining portion of the prediction
memcpy(&child->prediction[0], &parent->prediction[0], parent->window_start*sizeof(float));
for (int flow = child->window_end; flow < max_flows; ++flow)
child->prediction[flow] = 0;
// Fill out the solution
child->sequence = parent->sequence;
child->sequence.push_back(nuc_int_to_char[nuc]);
//calculate starting base position for each flow
child->base_pos = parent->base_pos;
child->base_pos++;
child->flow2base_pos = parent->flow2base_pos;
for(int flow_inter = parent->flow+1; flow_inter < child->flow; flow_inter++){
child->flow2base_pos[flow_inter] = child->flow2base_pos[parent->flow];
}
child->flow2base_pos[child->flow] = child->base_pos;
}
// ------------------------------------------
// Step 5. Check if the selected path is in fact the best path so far
// Computing sequence squared distance
float sum_of_squares = parent->residual_left_of_window;
for (int flow = parent->window_start; flow < max_flows; flow++) {
float residual = 0;
if(pm_model_enabled_==false){
residual = read.normalized_measurements[flow] - parent->prediction[flow];
}
else{
int hp_length = 0;
if(flow==0) hp_length = parent->flow2base_pos[0];
else hp_length = parent->flow2base_pos[flow] - parent->flow2base_pos[flow-1];
if(hp_length<0) hp_length = 0;
residual = read.normalized_measurements[flow] - (parent->prediction[flow]
* (*As_)[flow][flow_order_.int_at(flow)][hp_length]
+ (*Bs_)[flow][flow_order_.int_at(flow)][hp_length]);
}
sum_of_squares += residual * residual;
}
// Updating best path
if (sum_of_squares < sum_of_squares_upper_bound) {
read.prediction.swap(parent->prediction);
read.sequence.swap(parent->sequence);
sum_of_squares_upper_bound = sum_of_squares;
}
parent->in_use = false;
space_on_stack++;
} // main decision loop
}
int crypto_aead_decrypt(
unsigned char *m,unsigned long long *mlen,
unsigned char *nsec,
const unsigned char *c,unsigned long long clen,
const unsigned char *ad,unsigned long long adlen,
const unsigned char *npub,
const unsigned char *k
)
{
/*
... generating a plaintext m[0],m[1],...,m[*mlen-1]
... and secret message number nsec[0],nsec[1],...
... from a ciphertext c[0],c[1],...,c[clen-1]
... and associated data ad[0],ad[1],...,ad[adlen-1]
... and public message number npub[0],npub[1],...
... and secret key k[0],k[1],...
...
*/
unsigned char V[StateSize]; //the state represented in Elements
//Temp variables represented as elements
unsigned char key[KeySizeElements];
unsigned char nonce[NonceSizeElements];
unsigned char tag[TagSizeElements];
unsigned char data[RateSizeElements];
unsigned char cipher[RateSizeElements];
unsigned char IV[StateSize]; //temp state represented in Elements
if((clen<(TagSizeBytes+RateSizeBytes))&&(clen!=TagSizeBytes)) return -1; //invalid ciphertext
//a temporary array for message
unsigned char *mT = (unsigned char*) malloc(clen-TagSizeBytes);
if (mT == NULL) return -2; // memory allocation failure
Bytes2Element(key,k,0,KeySizeElements); //representing k as elements
Bytes2Element(nonce,npub,0,NonceSizeElements); //representing npub as elements
// IV <-- O||K
InitializeState(IV,key);
// Treating the nonce as the first part of the associated data
for(int i=0; i<(NonceSizeElements/RateSizeElements); i++)
{
// IV <-- p_1 (N[i]^IV_r || IV_c)
for(int j=0; j<RateSizeElements; j++)
IV[j]^=nonce[RateSizeElements*i+j];
p_1(IV);
}
if(adlen!=0)
{
// for i=1 to u-1 (except the last block which needs padding)
if(adlen>RateSizeBytes)
{
for(unsigned long long i=0; i<adlen-RateSizeBytes; i=i+RateSizeBytes)
{
Bytes2Element(data,ad,i,RateSizeElements); // representing a block of ad as elements
// IV <-- p_1 (A[i]^IV_r || IV_c)
for(int j=0; j<RateSizeElements; j++)
IV[j]^=data[j];
p_1(IV);
}
}
// IV <-- A[u]^IV_r || IV_c
int l = adlen%RateSizeBytes;
if(l==0){ //do padding with spill over
Bytes2Element(data,ad,adlen-RateSizeBytes,RateSizeElements); // representing last block of ad as elements
for(int i=0; i<RateSizeElements; i++)
IV[i]^=data[i];
IV[RateSizeElements]^=16; //padding spills over to capacity
}
else{ //do standard padding
unsigned char dataP[RateSizeBytes];
for(int i=0; i< l; i++)
dataP[i]=ad[adlen-l+i];
dataP[l]=128; //padding over bytes
for(int i=l+1; i<RateSizeBytes; i++)
dataP[i]=0;
Bytes2Element(data,dataP,0,RateSizeElements); // representing last padded block of ad as elements
for(int i=0; i<RateSizeElements; i++)
IV[i]^=data[i];
}
// IV <-- p_1(IV)
p_1(IV);
}
// IV <-- IV ^ (0..01)
IV[StateSize-1]^=1;
unsigned char tagT[TagSizeBytes];
if(clen==TagSizeBytes){ //ciphertext length and hence the message length is 0
*mlen=0;
//do standard padding
unsigned char dataP[RateSizeBytes];
dataP[0]=128; //padding over bytes
for(int i=1; i<RateSizeBytes; i++)
dataP[i]=0;
Bytes2Element(data,dataP,0,RateSizeElements); // representing last block of m as elements
// V <-- p_1 (M[1]^V_r || V_c)
for(int i=0; i<RateSizeElements; i++)
IV[i]^=data[i];
p_1(IV);
// T <-- (V_c)_c/2 ^ K
for(int i=0; i<TagSizeElements; i++)
tag[i]=IV[RateSizeElements+i]^key[i];
Element2Bytes(tagT,tag,0,TagSizeBytes) ;
unsigned char cor=0;
for(int i=0; i<TagSizeBytes; i++) //check if tags match
cor = cor || (tagT[i]^c[i]);
free(mT);
if(cor==0) return 0;
else return -1;
}
else{
// V <-- p_1_inv(C[w] || K^T)
Bytes2Element(cipher,c,clen-(TagSizeBytes+RateSizeBytes),RateSizeElements);
Bytes2Element(tag,c,clen-(TagSizeBytes),TagSizeElements);
for(int i=0; i<RateSizeElements; i++)
V[i]=cipher[i];
for(int i=0; i<CapacitySize; i++)
V[RateSizeElements+i]=key[i]^tag[i];
p_1_inv(V);
if(clen==(TagSizeBytes+RateSizeBytes)){ //ciphertext is one block only hence plaintext length is less then or equal to RateSize
// V <-- V ^ IV
for(int i=0; i<StateSize; i++)
V[i] ^= IV[i];
unsigned char cor=0;
for(int i=RateSizeElements+1; i<StateSize; i++) //check if IV_c matches with V_c
cor = cor || V[i];
cor = cor || (!((V[RateSizeElements]==0) || (V[RateSizeElements]==16)));
if (cor==0)
{
for(int i=0; i<RateSizeElements; i++)
data[i]=V[i];
Element2Bytes(mT,data,0,RateSizeBytes); //representing m as bytes
if(V[RateSizeElements]==16) *mlen=(unsigned long long) RateSizeBytes;
else{
unsigned char match=0;
for(int i=0; i<RateSizeBytes; i++) {
match |= (mT[i]==128);
}
if(match==0) { free(mT); return -1; }
for(int i=RateSizeBytes-1; i>=0; i--)
if(mT[i]==128) { *mlen=(unsigned long long) i; break;}
}
for(unsigned long long i=0; i<*mlen; i++)
m[i]=mT[i];
free(mT);
return 0;
}
else { free(mT); return -1; }
}
else{
*mlen=clen-TagSizeBytes;
//for i=w (tha last block)
int l = clen%RateSizeBytes;
if (l==0){
//M[w] <-- |V_r|_l^C[w-1]
Bytes2Element(cipher,c,clen-(TagSizeBytes+2*RateSizeBytes),RateSizeElements);
for(int i=0; i<RateSizeElements; i++)
data[i]=V[i]^cipher[i];
Element2Bytes(mT,data,clen-(TagSizeBytes+RateSizeBytes),RateSizeBytes); //representing last block m as bytes
//V <-- V^M[w]10*
for(int i=0; i<(RateSizeElements); i++)
V[i]^=data[i];
V[RateSizeElements]^=16;
}
else{
//M[w] <-- |V_r|_l^C[w-1]
unsigned char cipherP[RateSizeBytes];
unsigned char mtmp[RateSizeBytes];
Element2Bytes(cipherP,V,0,RateSizeBytes); //representing last block m as bytes
for(int i=0; i<l; i++){
mT[(clen-TagSizeBytes)-l+i]=cipherP[i]^c[clen-(TagSizeBytes+RateSizeBytes+l)+i];
mtmp[i]=mT[(clen-TagSizeBytes)-l+i];
}
mtmp[l]=128;
for(int i=l+1; i<RateSizeBytes; i++)
mtmp[i]=0;
Bytes2Element(data,mtmp,0,RateSizeElements);
//V <-- V^M[w]10*
for(int i=0; i<RateSizeElements; i++)
V[i]^=data[i];
}
l = (l==0) ? RateSizeBytes : l;
//for i=w-1 to 2
for(unsigned long long i=((clen-TagSizeBytes)-l); i>RateSizeBytes; i=i-5){
// V <-- p_1_inv(V)
p_1_inv(V);
// M[i] <-- C[i-1]^V_r
unsigned char mtmp[RateSizeElements];
Bytes2Element(data,c,i-2*RateSizeBytes,RateSizeElements);
for(int j=0; j<RateSizeElements; j++)
mtmp[j]=data[j]^V[j];
Element2Bytes(mT,mtmp,i-RateSizeBytes,RateSizeBytes); //representing last block m as bytes
// V <-- C[i-1] || V_c
for(int j=0; j<RateSizeElements; j++)
V[j]=data[j];
}
//for i=1
// V <-- p_1_inv(V)
p_1_inv(V);
// M[i] <-- C[i-1]^V_r
for(int i=0; i<RateSizeElements; i++)
data[i]=IV[i]^V[i];
Element2Bytes(mT,data,0,RateSizeBytes); //representing last block m as bytes
//check if IV_c matches with V_c
for(int j=0; j<RateSizeElements; j++)
V[j]^=data[j];
unsigned char cor=0;
for(int i=RateSizeElements; i<StateSize; i++)
cor = cor || (V[i]^IV[i]);
if(cor==0) {
for(unsigned long long i=0; i<*mlen; i++)
m[i]=mT[i];
}
free(mT);
if (cor == 0) return 0;
else return -1;
}
}
}
int crypto_aead_decrypt(
unsigned char *m,unsigned long long *mlen,
unsigned char *nsec,
const unsigned char *c,unsigned long long clen,
const unsigned char *ad,unsigned long long adlen,
const unsigned char *npub,
const unsigned char *k
)
{
/*
... generating a plaintext m[0],m[1],...,m[*mlen-1]
... and secret message number nsec[0],nsec[1],...
... from a ciphertext c[0],c[1],...,c[clen-1]
... and associated data ad[0],ad[1],...,ad[adlen-1]
... and public message number npub[0],npub[1],...
... and secret key k[0],k[1],...
...
*/
unsigned char V[StateSize]; //the state represented in Elements
//Temp variables represented as elements
unsigned char key[KeySizeElements];
unsigned char nonce[NonceSizeElements];
unsigned char tag[TagSizeElements];
unsigned char data[RateSizeElements];
unsigned char cipher[RateSizeElements];
*mlen=clen-TagSizeBytes;
if(clen<TagSizeBytes) return -1; //invalid ciphertext
//a temporary array for message
unsigned char *mT = (unsigned char*) malloc(clen-TagSizeBytes);
if (mT == NULL) return -2; // memory allocation failure
Bytes2Element(key,k,0,KeySizeElements); //representing k as elements
Bytes2Element(nonce,npub,0,NonceSizeElements); //representing npub as elements
// V <-- p_1(O||K||N)
InitializeState(V,key,nonce);
p_1(V);
if(adlen!=0)
{
// for i=1 to u-1
if(adlen>RateSizeBytes)
{
for(unsigned long long i=0; i<adlen-RateSizeBytes; i=i+RateSizeBytes)
{
Bytes2Element(data,ad,i,RateSizeElements); // representing a block of ad as elements
// V <-- p_4 (A[i]^V_r || V_c)
for(int j=0; j<RateSizeElements; j++)
V[j]^=data[j];
p_4(V);
}
}
// V <-- A[u]^v_r || V_c
int l = adlen%RateSizeBytes;
if(l==0) { //do padding with spill over
Bytes2Element(data,ad,adlen-RateSizeBytes,RateSizeElements);
for(int i=0; i<RateSizeElements; i++)
V[i]^=data[i];
V[RateSizeElements]^=16; //padding spills over to capacity
}
else { //do standard padding
unsigned char dataP[RateSizeBytes];
for(int i=0; i< l; i++)
dataP[i]=ad[adlen-l+i];
dataP[l]=128; //padding over bytes
for(int i=l+1; i<RateSizeBytes; i++)
dataP[i]=0;
Bytes2Element(data,dataP,0,RateSizeElements);
for(int i=0; i<RateSizeElements; i++)
V[i]^=data[i];
}
// V <-- p_1(V)
p_1(V);
}
//for i=1 to w-1 (except the last block which needs padding)
if(clen-TagSizeBytes>RateSizeBytes)
{
for(unsigned long long i=0; i<clen-(TagSizeBytes+RateSizeBytes); i=i+RateSizeBytes)
{
Bytes2Element(cipher,c,i,RateSizeElements); //representing a block of c as elements
// M[i] <-- C[i]^V_r
for(int j=0; j<RateSizeElements; j++)
data[j]=V[j]^cipher[j];
Element2Bytes(mT,data,i,RateSizeBytes); //representing m as bytes
//V <-- p_1 (C[i] || V_c)
for(int j=0; j<RateSizeElements; j++)
V[j]=cipher[j];
p_1(V);
}
}
//for i=w
int l = clen%RateSizeBytes;
// M[w] <-- C[w]^V_r
// V <-- M[w]||10* ^ V
if((l==0) && ((clen-TagSizeBytes)!=0)) { //do padding with spill over
Bytes2Element(cipher,c,clen-(TagSizeBytes+RateSizeBytes),RateSizeElements);
for(int i=0; i<RateSizeElements; i++) {
data[i]=V[i]^cipher[i];
V[i]=cipher[i];
}
Element2Bytes(mT,data,clen-(TagSizeBytes+RateSizeBytes),RateSizeBytes); //representing last block m as bytes
V[RateSizeElements]^=16; //padding spills over to capacity
}
else { //do standard padding
unsigned char cipherP[RateSizeElements];
unsigned char mtmp[RateSizeBytes];
for(int i=0; i< RateSizeElements; i++)
cipherP[i]=V[i];
Element2Bytes(mtmp,cipherP,0,RateSizeBytes);
for(int i=0; i< l; i++)
mT[clen-TagSizeBytes-l+i]=mtmp[i]^c[clen-TagSizeBytes-l+i];
unsigned char dataP[RateSizeBytes];
for(int i=0; i< l; i++)
dataP[i]=mT[clen-TagSizeBytes-l+i];
dataP[l]=128; //padding over bytes
for(int i=l+1; i<RateSizeBytes; i++)
dataP[i]=0;
Bytes2Element(data,dataP,0,RateSizeElements);
for(int i=0; i<RateSizeElements; i++)
V[i]^=data[i];
}
//V <-- p_1 (V)
p_1(V);
// T <-- (V_c)_c/2 ^ K
for(int i=0; i<TagSizeElements; i++)
tag[i]=V[RateSizeElements+i]^key[i];
unsigned char tagT[TagSizeBytes];
Element2Bytes(tagT,tag,0,TagSizeBytes) ;
unsigned char cor=0;
for(int i=0; i<TagSizeBytes; i++) //check if tags match
cor = cor || (tagT[i]^c[clen-TagSizeBytes+i]);
if(cor==0) //if tags match release the message
for(unsigned long long i=0; i<clen-TagSizeBytes; i++)
m[i]=mT[i];
free(mT);
if(cor==0) return 0;
else return -1;
}
int crypto_aead_encrypt(
unsigned char *c,unsigned long long *clen,
const unsigned char *m,unsigned long long mlen,
const unsigned char *ad,unsigned long long adlen,
const unsigned char *nsec,
const unsigned char *npub,
const unsigned char *k
)
{
/*...
... generating a ciphertext c[0],c[1],...,c[*clen-1]
... from a plaintext m[0],m[1],...,m[mlen-1]
... and associated data ad[0],ad[1],...,ad[adlen-1]
... and secret message number nsec[0],nsec[1],...
... and public message number npub[0],npub[1],...
... and secret key k[0],k[1],...
...*/
unsigned char V[StateSize]; //the state represented in Elements
//Temp variables represented as elements
unsigned char key[KeySizeElements];
unsigned char nonce[NonceSizeElements];
unsigned char tag[TagSizeElements];
unsigned char data[RateSizeElements];
unsigned char cipher[RateSizeElements];
Bytes2Element(key,k,0,KeySizeElements); //representing k as elements
Bytes2Element(nonce,npub,0,NonceSizeElements); //representing npub as elements
// V <-- p_1(O||K||N)
InitializeState(V,key,nonce);
p_1(V);
if(adlen!=0)
{
// for i=1 to u-1
if(adlen>RateSizeBytes)
{
for(unsigned long long i=0; i<adlen-RateSizeBytes; i=i+RateSizeBytes)
{
Bytes2Element(data,ad,i,RateSizeElements); // representing a block of ad as elements
// V <-- p_4 (A[i]^V_r || V_c)
for(int j=0; j<RateSizeElements; j++)
V[j]^=data[j];
p_4(V);
}
}
// V <-- A[u]^v_r || V_c
int l = adlen%RateSizeBytes;
if(l==0) { //do padding with spill over
Bytes2Element(data,ad,adlen-RateSizeBytes,RateSizeElements); // representing last block of ad as elements
for(int i=0; i<RateSizeElements; i++)
V[i]^=data[i];
V[RateSizeElements]^=16; //padding spills over to capacity
}
else { //do standard padding
unsigned char dataP[RateSizeBytes];
for(int i=0; i< l; i++)
dataP[i]=ad[adlen-l+i];
dataP[l]=128; //padding over bytes
for(int i=l+1; i<RateSizeBytes; i++)
dataP[i]=0;
Bytes2Element(data,dataP,0,RateSizeElements); // representing last padded block of ad as elements
for(int i=0; i<RateSizeElements; i++)
V[i]^=data[i];
}
// V <-- p_1(V)
p_1(V);
}
//for i=1 to w-1 (except the last block which needs padding)
if(mlen>RateSizeBytes)
{
for(unsigned long long i=0; i<mlen-RateSizeBytes; i=i+RateSizeBytes)
{
Bytes2Element(data,m,i,RateSizeElements); //representing a block of m as elements
// C[i] <-- M[i]^V_r
for(int j=0; j<RateSizeElements; j++)
cipher[j]=V[j]^data[j];
Element2Bytes(c,cipher,i,RateSizeBytes); //representing a block of c as bytes
//V <-- p_1 (C[i] || V_c)
for(int j=0; j<RateSizeElements; j++)
V[j]=cipher[j];
p_1(V);
}
}
//for i=w
int l = mlen%RateSizeBytes;
// C[i] <-- M[i]^V_r
if((l==0) && (mlen!=0)) { //do padding with spill over
Bytes2Element(data,m,mlen-RateSizeBytes,RateSizeElements); // representing last block of m as elements
for(int i=0; i<RateSizeElements; i++)
cipher[i]=V[i]^data[i];
Element2Bytes(c,cipher,mlen-RateSizeBytes,RateSizeBytes); //representing last block of c as bytes
V[RateSizeElements]^=16; //padding spills over to capacity
}
else { //do standard padding
unsigned char dataP[RateSizeBytes];
for(int i=0; i< l; i++)
dataP[i]=m[mlen-l+i];
dataP[l]=128; //padding over bytes
for(int i=l+1; i<RateSizeBytes; i++)
dataP[i]=0;
Bytes2Element(data,dataP,0,RateSizeElements); // representing last block of m as elements
for(int i=0; i<RateSizeElements; i++)
cipher[i]=V[i]^data[i];
unsigned char ct[RateSizeBytes];
Element2Bytes(ct,cipher,0,RateSizeBytes); //representing last block of c as bytes
for(int i=0; i< l; i++)
c[mlen-l+i]=ct[i]; //storing only l bytes of c
}
//V <-- p_1 (C[i] || V_c)
for(int j=0; j<RateSizeElements; j++)
V[j]=cipher[j];
p_1(V);
// T <-- (V_c)_c/2 ^ K
for(int i=0; i<TagSizeElements; i++)
tag[i]=V[RateSizeElements+i]^key[i];
Element2Bytes(c,tag,mlen,TagSizeBytes) ;
*clen=mlen+TagSizeBytes;
return 0;
}
void DPTreephaser::ComputeQVmetrics(BasecallerRead& read)
{
static const char nuc_int_to_char[5] = "ACGT";
//TODO: remove this assignment as it has been moved underSetDataAndKeyNormalize or SetDataAndKeyNormalizeNew
read.state_inphase.assign(flow_order_.num_flows(), 1);
read.state_total.assign(flow_order_.num_flows(), 1);
if (read.sequence.empty())
return;
read.penalty_mismatch.assign(read.sequence.size(), 0);
read.penalty_residual.assign(read.sequence.size(), 0);
TreephaserPath *parent = &path_[0];
TreephaserPath *children[4] = { &path_[1], &path_[2], &path_[3], &path_[4] };
InitializeState(parent);
float recent_state_inphase = 1;
float recent_state_total = 1;
// main loop for base calling
for (int solution_flow = 0, base = 0; solution_flow < flow_order_.num_flows(); ++solution_flow) {
for (; base < (int)read.sequence.size() and read.sequence[base] == flow_order_[solution_flow]; ++base) {
float penalty[4] = { 0, 0, 0, 0 };
int called_nuc = 0;
for (int nuc = 0; nuc < 4; nuc++) {
TreephaserPath *child = children[nuc];
AdvanceState(child, parent, nuc_int_to_char[nuc], flow_order_.num_flows());
if (nuc_int_to_char[nuc] == flow_order_[solution_flow])
called_nuc = nuc;
// Apply easy termination rules
if (child->flow >= flow_order_.num_flows()) {
penalty[nuc] = 25; // Mark for deletion
continue;
}
if (parent->last_hp >= kMaxHP) {
penalty[nuc] = 25; // Mark for deletion
continue;
}
if ((int)parent->sequence.size() >= (2 * flow_order_.num_flows() - 10)) {
penalty[nuc] = 25; // Mark for deletion
continue;
}
for (int flow = parent->window_start; flow < child->window_end; ++flow) {
float residual = read.normalized_measurements[flow] - child->prediction[flow];
if (residual <= 0 or flow < child->flow)
penalty[nuc] += residual*residual;
}
} //looping over nucs
// find current incorporating base
assert(children[called_nuc]->flow == solution_flow);
recent_state_inphase = children[called_nuc]->state[solution_flow];
recent_state_total = 0;
for (int flow = children[called_nuc]->window_start; flow < children[called_nuc]->window_end; ++flow)
recent_state_total += children[called_nuc]->state[flow];
// Get delta penalty to next best solution
read.penalty_mismatch[base] = -1; // min delta penalty to earlier base hypothesis
read.penalty_residual[base] = 0;
if (solution_flow - parent->window_start > 0)
read.penalty_residual[base] = penalty[called_nuc] / (solution_flow - parent->window_start);
for (int nuc = 0; nuc < 4; ++nuc) {
if (nuc == called_nuc)
continue;
float penalty_mismatch = penalty[called_nuc] - penalty[nuc];
read.penalty_mismatch[base] = max(read.penalty_mismatch[base], penalty_mismatch);
}
// Fill out the remaining portion of the prediction
for (int flow = 0; flow < parent->window_start; ++flow)
children[called_nuc]->prediction[flow] = parent->prediction[flow];
for (int flow = children[called_nuc]->window_end; flow < flow_order_.num_flows(); ++flow)
children[called_nuc]->prediction[flow] = 0;
// Called state is the starting point for next base
TreephaserPath *swap = parent;
parent = children[called_nuc];
children[called_nuc] = swap;
}
read.state_inphase[solution_flow] = max(recent_state_inphase, 0.01f);
read.state_total[solution_flow] = max(recent_state_total, 0.01f);
}
read.prediction.swap(parent->prediction);
}
Eigen::VectorXi TvlqrControl::GetControl(const mav_pose_t *msg) {
if (current_trajectory_ == NULL) {
cerr << "Warning: NULL trajectory in GetControl." << endl;
return converter_->GetTrimCommands();
}
//std::cout << "in GetControl" << std::endl;
// check to see if this is the first state we've gotten along this trajectory
if (state_initialized_ == false) {
InitializeState(msg);
}
Eigen::VectorXd state_minus_init = GetStateMinusInit(msg);
/*
printf("BEFORE\troll: %f\tpitch: %f\tyaw: %f\n", state_minus_init(3), state_minus_init(4), state_minus_init(5));
// unwrap angles
state_minus_init(3) = AngleUnwrap(state_minus_init(3), last_state_(3));
state_minus_init(4) = AngleUnwrap(state_minus_init(4), last_state_(4));
state_minus_init(5) = AngleUnwrap(state_minus_init(5), last_state_(5));
printf("AFTER\troll: %f\tpitch: %f\tyaw: %f\n", state_minus_init(3), state_minus_init(4), state_minus_init(5));
last_state_ = state_minus_init;
*/
double t_along_trajectory;
// check for TILQR case
if (current_trajectory_->IsTimeInvariant()) {
t_along_trajectory = 0;
} else {
t_along_trajectory = GetTNow();
}
if (t_along_trajectory <= current_trajectory_->GetMaxTime()) {
Eigen::VectorXd x0 = current_trajectory_->GetState(t_along_trajectory);
Eigen::MatrixXd gain_matrix = current_trajectory_->GetGainMatrix(t_along_trajectory);
Eigen::VectorXd state_error = state_minus_init - x0;
//cout << "state error = " << endl << state_error << endl;
Eigen::VectorXd additional_control_action = gain_matrix * state_error;
//cout << "additional control action = " << endl << additional_control_action << endl;
//cout << "t = " << t_along_trajectory << endl;
//cout << "gain" << endl << gain_matrix << endl << "state_error" << endl << state_error << endl << "additional" << endl << additional_control_action << endl;
Eigen::VectorXd command_in_rad = current_trajectory_->GetUCommand(t_along_trajectory) + additional_control_action;
//cout << "command_in_rad" << endl << command_in_rad << endl;
return converter_->RadiansToServoCommands(command_in_rad);
} else {
// we are past the max time, return stabilizing controller
SetTrajectory(stable_controller_);
return GetControl(msg);
//return converter_->GetTrimCommands();
}
}
void Sampler::Initialize(lib_corpora::MlSeqCorpus* corpus) {
corpus_ = corpus;
InitializeState(FLAGS_output_location, FLAGS_random_init);
InitializeTemp();
}
int main(int argc, const char *argv[])
{
if (argc == 1) // print usage instructions
{
std::cout << "Usage: \n"
<< "\t--datadir <dirname> REQUIRED [place to store db shards]\n"
<< "\t--nodestate <filename> REQUIRED [current node state]\n"
<< "\t--clustermembers <filename> REQUIRED [list of cluster members]\n"
<< "\t--ownershipmap <filename> REQUIRED [list of partitions owned by current node]\n"
<< "\t--partitionmap <filename> REQUIRED [mapping of partitions to nodes]\n"
<< "\t--log <filename> REQUIRED [log file for recovery]\n"
<< "\t--join OPTIONAL\n"
<< "\t--recover OPTIONAL\n"
<< "\t--debug OPTIONAL [start sending fake data to other cluster members]\n"
<< "\t--size OPTIONAL [mbytes]"
// << "\t--name OPTIONAL [give the node a custom name that can be reached, IP address]"
<< std::endl;
return 0;
}
std::string logfile;
int i;
bool join = false, recover = false, debug = false;
if ((i = ArgPos("--datadir", argc, argv, true)) > 0)
g_db_files_dirname = std::string(argv[i+1]);
if ((i = ArgPos("--nodestate", argc, argv, true)) > 0)
g_current_node_state_filename = std::string(argv[i+1]);
if ((i = ArgPos("--clustermembers", argc, argv, true)) > 0)
g_cluster_member_list_filename = std::string(argv[i+1]);
if ((i = ArgPos("--ownershipmap", argc, argv, true)) > 0)
g_owned_partition_state_filename = std::string(argv[i+1]);
if ((i = ArgPos("--partitionmap", argc, argv, true)) > 0)
g_cached_partition_map_filename = std::string(argv[i+1]);
if ((i = ArgPos("--log", argc, argv, true)) > 0)
logfile = std::string(argv[i+1]);
if ((i = ArgPos("--join", argc, argv, false)) > 0)
join = true;
if ((i = ArgPos("--recover", argc, argv, false)) > 0)
recover = true;
if ((i = ArgPos("--debug", argc, argv, false)) > 0)
debug = true;
if ((i = ArgPos("--size", argc, argv, true)) > 0)
g_local_disk_limit_in_bytes = atoi(argv[i+1]) * 1024 * 1024;
else
g_local_disk_limit_in_bytes = 512 * 1024 * 1024;
std::cout << "DB directory : " << g_db_files_dirname << std::endl;
std::cout << "Node state file : " << g_current_node_state_filename << std::endl;
std::cout << "Cluster members file : " << g_cluster_member_list_filename << std::endl;
std::cout << "Ownership map file : " << g_owned_partition_state_filename << std::endl;
std::cout << "Partition-node map file : " << g_cached_partition_map_filename << std::endl;
std::cout << "Log file : " << logfile << std::endl;
if (join && recover)
{
perror("cannot join and recover\n");
exit(0);
}
if (g_db_files_dirname == "")
{
perror("must specify a directory for database files\n");
exit(0);
}
if (g_current_node_state_filename == "")
{
perror("must specify a filename for persisting node state\n");
exit(0);
}
if (g_cluster_member_list_filename == "")
{
perror("must specify a file for list of cluster members\n");
exit(0);
}
if (g_owned_partition_state_filename == "")
{
perror("must specify a file for owned partition state\n");
exit(0);
}
if (g_cached_partition_map_filename == "")
{
perror("must specify a file for partition map\n");
exit(0);
}
if (logfile == "")
{
perror("must specify a file for log\n");
exit(0);
}
if (join)
{
JoinInitState();
}
else if (recover)
{
RecoverInitState(logfile);
}
else
{
InitializeState();
}
edisense_comms::Member member;
Server server;
member.start(&server);
if (join)
{
JoinFinishInit(&member);
}
std::thread async_put_thread(RetryPutDaemon, &member, 15); // 15 seconds
std::thread rebalance_thread(LoadBalanceDaemon, &member, 60, logfile, recover); // 1 minutes
std::thread gc_thread(GarbageCollectDaemon, 60 * 60 * 12); // 12 hrs
std::thread db_transfer_thread(DBTransferServerDaemon);
if (debug) // simulate data
{
for (int j = 1; j <= 50; j++)
{
std::thread simulate_put_thread(SimulatePutDaemon, &member, 1, j);
simulate_put_thread.detach();
}
}
gc_thread.join();
rebalance_thread.join();
db_transfer_thread.join();
async_put_thread.join();
member.stop();
}
void Base::SwitchToState(MainState newState)
{
ShutdownState(m_currentState);
m_currentState = newState;
InitializeState(m_currentState);
}
/*
*
* Entry point into program
*
*/
int main()
{
RAM_DATA_BYTE state[BLOCK_SIZE];
RAM_DATA_BYTE key[KEY_SIZE];
RAM_DATA_BYTE roundKeys[ROUND_KEYS_SIZE];
InitializeDevice();
InitializeState(state);
#if defined(DEBUG) && (DEBUG_LOW == (DEBUG_LOW & DEBUG))
DisplayVerifyData(state, BLOCK_SIZE, PLAINTEXT_NAME);
#endif
InitializeKey(key);
#if defined(DEBUG) && (DEBUG_MEDIUM == (DEBUG_MEDIUM & DEBUG))
DisplayVerifyData(key, KEY_SIZE, KEY_NAME);
#endif
#if defined(DEBUG) && (DEBUG_HIGH == (DEBUG_HIGH & DEBUG))
DisplayData(roundKeys, ROUND_KEYS_SIZE, ROUND_KEYS_NAME);
#endif
BEGIN_ENCRYPTION_KEY_SCHEDULE();
RunEncryptionKeySchedule(key, roundKeys);
END_ENCRYPTION_KEY_SCHEDULE();
#if defined(DEBUG) && (DEBUG_MEDIUM == (DEBUG_MEDIUM & DEBUG))
DisplayVerifyData(key, KEY_SIZE, KEY_NAME);
#endif
#if defined(DEBUG) && (DEBUG_HIGH == (DEBUG_HIGH & DEBUG))
DisplayData(roundKeys, ROUND_KEYS_SIZE, ROUND_KEYS_NAME);
#endif
#if defined(DEBUG) && (DEBUG_LOW == (DEBUG_LOW & DEBUG))
DisplayVerifyData(state, BLOCK_SIZE, PLAINTEXT_NAME);
#endif
BEGIN_ENCRYPTION();
Encrypt(state, roundKeys);
END_ENCRYPTION();
#if defined(DEBUG) && (DEBUG_LOW == (DEBUG_LOW & DEBUG))
DisplayVerifyData(state, BLOCK_SIZE, CIPHERTEXT_NAME);
#endif
BEGIN_DECRYPTION_KEY_SCHEDULE();
RunDecryptionKeySchedule(key, roundKeys);
END_DECRYPTION_KEY_SCHEDULE();
#if defined(DEBUG) && (DEBUG_MEDIUM == (DEBUG_MEDIUM & DEBUG))
DisplayVerifyData(key, KEY_SIZE, KEY_NAME);
#endif
#if defined(DEBUG) && (DEBUG_HIGH == (DEBUG_HIGH & DEBUG))
DisplayData(roundKeys, ROUND_KEYS_SIZE, ROUND_KEYS_NAME);
#endif
#if defined(DEBUG) && (DEBUG_LOW == (DEBUG_LOW & DEBUG))
DisplayVerifyData(state, BLOCK_SIZE, CIPHERTEXT_NAME);
#endif
BEGIN_DECRYPTION();
Decrypt(state, roundKeys);
END_DECRYPTION();
#if defined(DEBUG) && (DEBUG_LOW == (DEBUG_LOW & DEBUG))
DisplayVerifyData(state, BLOCK_SIZE, PLAINTEXT_NAME);
#endif
DONE();
StopDevice();
return 0;
}
