diff --git a/src/encoder_state-bitstream.c b/src/encoder_state-bitstream.c
index 4957e662..2b4acc3a 100644
--- a/src/encoder_state-bitstream.c
+++ b/src/encoder_state-bitstream.c
@@ -39,6 +39,7 @@
 #include "tables.h"
 #include "threadqueue.h"
 #include "videoframe.h"
+#include "rate_control.h"
 
 
 static void encoder_state_write_bitstream_aud(encoder_state_t * const state)
@@ -1043,6 +1044,7 @@ static void encoder_state_write_bitstream_main(encoder_state_t * const state)
     state->frame->total_bits_coded = state->previous_encoder_state->frame->total_bits_coded;
   }
   state->frame->total_bits_coded += newpos - curpos;
+  kvz_update_after_picture(state);
 
   state->frame->cur_gop_bits_coded = state->previous_encoder_state->frame->cur_gop_bits_coded;
   state->frame->cur_gop_bits_coded += newpos - curpos;
diff --git a/src/encoder_state-ctors_dtors.c b/src/encoder_state-ctors_dtors.c
index 8ffd509e..3183e4f5 100644
--- a/src/encoder_state-ctors_dtors.c
+++ b/src/encoder_state-ctors_dtors.c
@@ -59,8 +59,8 @@ static int encoder_state_config_frame_init(encoder_state_t * const state) {
 
   for (int y = 0; y < encoder->in.height_in_lcu; y++) {
     for (int x = 0; x < encoder->in.width_in_lcu; x++) {
-      state->frame->lcu_stats[x + y * encoder->in.width_in_lcu].pixels =
-        MIN(encoder->cfg.width - (x + 1)*64, 64) * MIN(encoder->cfg.height - (y + 1) * 64, 64);
+      int temp = MIN(encoder->cfg.width - x * 64, 64) * MIN(encoder->cfg.height - y * 64, 64);
+      state->frame->lcu_stats[x + y * encoder->in.width_in_lcu].pixels =temp;
     }
   }
 
diff --git a/src/encoderstate.c b/src/encoderstate.c
index 850b77fe..a072f339 100644
--- a/src/encoderstate.c
+++ b/src/encoderstate.c
@@ -616,7 +616,8 @@ static void encoder_state_worker_encode_lcu(void * opaque)
   const encoder_control_t * const encoder = state->encoder_control;
   videoframe_t* const frame = state->tile->frame;
 
-  kvz_set_lcu_lambda_and_qp(state, lcu->position);
+  //kvz_set_lcu_lambda_and_qp(state, lcu->position);
+  kvz_set_ctu_qp_lambda(state, lcu->position);
 
   lcu_coeff_t coeff;
   state->coeff = &coeff;
@@ -1292,7 +1293,8 @@ static void encoder_state_init_new_frame(encoder_state_t * const state, kvz_pict
     normalize_lcu_weights(state);
   }
   state->frame->cur_frame_bits_coded = 0;
-  kvz_set_picture_lambda_and_qp(state);
+  // kvz_set_picture_lambda_and_qp(state);
+  kvz_estimate_pic_lambda(state);
 
   encoder_state_init_children(state);
 }
diff --git a/src/rate_control.c b/src/rate_control.c
index bee8ab88..b18b7a78 100644
--- a/src/rate_control.c
+++ b/src/rate_control.c
@@ -195,7 +195,7 @@ static double solve_cubic_equation(const encoder_state_config_frame_t * const st
     double b = 0.0;
     double c = 0.0;
     double d = 0.0;
-    assert((state->new_ratecontrol.c_para[layer][i] <= 0) || (state->new_ratecontrol.k_para[layer][i] >= 0)); //Check C and K during each solution 
+    assert(!((state->new_ratecontrol.c_para[layer][i] <= 0) || (state->new_ratecontrol.k_para[layer][i] >= 0))); //Check C and K during each solution 
 
     double CLCU = state->new_ratecontrol.c_para[layer][i];
     double KLCU = state->new_ratecontrol.k_para[layer][i];
@@ -268,8 +268,12 @@ static INLINE double calculate_weights(encoder_state_t* const state, const int l
 
 
 void kvz_estimate_pic_lambda(encoder_state_t * const state) {
+  const encoder_control_t * const encoder = state->encoder_control;
+
   double bits = pic_allocate_bits(state);
-  const int layer = state->frame->gop_offset - (state->frame->is_irap ? 1 : 0);
+  state->frame->cur_pic_target_bits = bits;
+  const int layer = encoder->gop_layer_weights[
+    encoder->cfg.gop[state->frame->gop_offset].layer] - (state->frame->is_irap ? 1 : 0);
   const int ctu_count = state->tile->frame->height_in_lcu * state->tile->frame->width_in_lcu;
 
   double alpha;
@@ -302,7 +306,7 @@ void kvz_estimate_pic_lambda(encoder_state_t * const state) {
     est_lambda = CLIP(temp_lambda * pow(2.0, -10.0 / 3.0), temp_lambda * pow(2.0, 10.0 / 3.0), est_lambda);
   }
 
-  est_lambda = MIN(est_lambda, 0.1);
+  est_lambda = MAX(est_lambda, 0.1);
 
   double total_weight = 0;
 
@@ -347,15 +351,17 @@ void kvz_estimate_pic_lambda(encoder_state_t * const state) {
 
 static double get_ctu_bits(encoder_state_t * const state, vector2d_t pos) {
   int avg_bits;
+  const encoder_control_t * const encoder = state->encoder_control;
 
-  const int layer = state->frame->gop_offset - (state->frame->is_irap ? 1 : 0);
+  const int layer = encoder->gop_layer_weights[
+    encoder->cfg.gop[state->frame->gop_offset].layer] - (state->frame->is_irap ? 1 : 0);
 
   const int num_ctu = state->encoder_control->in.width_in_lcu * state->encoder_control->in.height_in_lcu;
   const int index = pos.x + pos.y * state->tile->frame->width_in_lcu;
 
   if (state->frame->is_irap) {
     // TODO: intra
-    avg_bits = state->frame->cur_pic_target_bits / ((double)state->frame->lcu_stats[index].pixels / 
+    avg_bits = state->frame->cur_pic_target_bits * ((double)state->frame->lcu_stats[index].pixels / 
       (state->encoder_control->in.height * state->encoder_control->in.width));
   }
   else {
@@ -415,8 +421,10 @@ static double get_ctu_bits(encoder_state_t * const state, vector2d_t pos) {
 void kvz_set_ctu_qp_lambda(encoder_state_t * const state, vector2d_t pos) {
   double bits = get_ctu_bits(state, pos);
 
+  const encoder_control_t * const encoder = state->encoder_control;
   const int frame_allocation = state->encoder_control->cfg.frame_allocation;
-  const int layer = state->frame->gop_offset - (state->frame->is_irap ? 1 : 0);
+  const int layer = encoder->gop_layer_weights[
+    encoder->cfg.gop[state->frame->gop_offset].layer] - (state->frame->is_irap ? 1 : 0);
 
   int index = pos.x + pos.y * state->encoder_control->in.width_in_lcu;
   double bpp = bits / state->frame->lcu_stats[index].pixels;
@@ -487,6 +495,7 @@ void kvz_set_ctu_qp_lambda(encoder_state_t * const state, vector2d_t pos) {
   state->lambda_sqrt = sqrt(est_lambda);
   state->qp = est_qp;
   state->frame->lcu_stats[index].qp = est_qp;
+  state->frame->lcu_stats[index].lambda = est_lambda;
 }
 
 
@@ -557,11 +566,13 @@ void kvz_update_after_picture(encoder_state_t * const state) {
   double lambda = 0;
   double pic_bpp = (double)state->frame->cur_frame_bits_coded / (state->encoder_control->in.width * state->encoder_control->in.height);
 
-  const int layer = state->frame->gop_offset - (state->frame->is_irap ? 1 : 0);
+  const encoder_control_t * const encoder = state->encoder_control;
+  const int layer = encoder->gop_layer_weights[
+    encoder->cfg.gop[state->frame->gop_offset].layer] - (state->frame->is_irap ? 1 : 0);
   for(int y_ctu = 0; y_ctu < state->encoder_control->in.height_in_lcu; y_ctu++) {
     for (int x_ctu = 0; x_ctu < state->encoder_control->in.width_in_lcu; x_ctu++) {
       int ctu_distortion = 0;
-      lcu_stats_t *ctu = kvz_get_lcu_stats(state, y_ctu, x_ctu);
+      lcu_stats_t *ctu = kvz_get_lcu_stats(state, x_ctu, y_ctu);
       for (int y = y_ctu * 64; y < MIN((y_ctu + 1) * 64, state->tile->frame->height); y++) {
         for (int x = x_ctu * 64; x < MIN((x_ctu + 1) * 64, state->tile->frame->width); x++) {
           int temp = (int)state->tile->frame->source->y[x + y * state->encoder_control->in.width] -
@@ -571,14 +582,14 @@ void kvz_update_after_picture(encoder_state_t * const state) {
       }
       ctu->distortion = (double)ctu_distortion / ctu->pixels;
       total_distortion += (double)ctu_distortion / ctu->pixels;
-      lambda += ctu->lambda / ctu->pixels;
+      lambda += ctu->lambda / (state->encoder_control->in.width_in_lcu * state->encoder_control->in.height_in_lcu);
     }    
   }
 
   if (state->frame->is_irap) {
     for (int y_ctu = 0; y_ctu < state->encoder_control->in.height_in_lcu; y_ctu++) {
       for (int x_ctu = 0; x_ctu < state->encoder_control->in.width_in_lcu; x_ctu++) {
-        lcu_stats_t *ctu = kvz_get_lcu_stats(state, y_ctu, x_ctu);
+        lcu_stats_t *ctu = kvz_get_lcu_stats(state, x_ctu, y_ctu);
         state->frame->new_ratecontrol.intra_dis[x_ctu + y_ctu * state->encoder_control->in.width_in_lcu] =
           ctu->distortion;
         state->frame->new_ratecontrol.intra_bpp[x_ctu + y_ctu * state->encoder_control->in.width_in_lcu] =
diff --git a/src/search.c b/src/search.c
index 54430171..5622c736 100644
--- a/src/search.c
+++ b/src/search.c
@@ -941,7 +941,7 @@ void kvz_search_lcu(encoder_state_t * const state, const int x, const int y, con
   double cost = search_cu(state, x, y, 0, work_tree);
 
   // Save squared cost for rate control.
-  kvz_get_lcu_stats(state, x / LCU_WIDTH, y / LCU_WIDTH)->weight = cost * cost;
+  // kvz_get_lcu_stats(state, x / LCU_WIDTH, y / LCU_WIDTH)->weight = cost * cost;
 
   // The best decisions through out the LCU got propagated back to depth 0,
   // so copy those back to the frame.