diff --git a/TODO.md b/TODO.md
index 0753e9e..e45095d 100644
--- a/TODO.md
+++ b/TODO.md
@@ -1 +1,2 @@
 [ ] Quaternions
+[ ] Draw rotated pixels in src order
diff --git a/rotation.cpp b/rotation.cpp
index 65461b7..7524431 100644
--- a/rotation.cpp
+++ b/rotation.cpp
@@ -113,6 +113,28 @@ struct Image {
     b_chan[index] = b;
   }
 
+  void set_pixel(Point const& p, uint8_t r, uint8_t g, uint8_t b)
+  {
+    this->set_pixel(p.x, p.y, r, g, b);
+  }
+
+  void get_pixel(APoint const& p, uint8_t& r, uint8_t& g, uint8_t& b) const
+  {
+    if (p.x < 0 || p.x >= (int) width || p.y < 0 || p.y >= (int) height)
+    {
+      // set out of domain pixels to black
+      r = 0;
+      g = 0;
+      b = 0;
+      return;
+    }
+
+    unsigned int const index = p.y * width + p.x;
+    r = r_chan[index];
+    g = g_chan[index];
+    b = b_chan[index];
+  }
+
   private:
     bool read_header(std::ifstream& istr)
     {
@@ -165,8 +187,8 @@ struct Image {
         return false;
       }
 
-      cout << "width: " << width << endl;
-      cout << "height: " << height << endl;
+      // cout << "width: " << width << endl;
+      // cout << "height: " << height << endl;
 
       return true;
     }
@@ -230,25 +252,38 @@ DPoint convert_grid_coord(Image const& img, Point const& p)
 double convert_radian(Image const& img, Point const& p, double const ratio)
 {
   DPoint centered = convert_grid_coord(img, p);
+  cout << "-> grid " << centered << endl;
   double const cos_value = centered.x * ratio;
+  cout << "cos = " << cos_value << endl;
   double const sin_value = - (centered.y * ratio);
+  cout << "sin = " << sin_value << endl;
   double angle = acos(cos_value);
   if (sin_value < 0)
-    angle = 2 * M_PI - angle;
+  {
+    cout << "mirror angle" << endl;
+    angle = (2 * M_PI) - angle;
+  }
+  cout << "radian = " << angle << endl;
+  cout << "acos = " << acos(cos_value) << endl;
+  cout << "asin = " << asin(sin_value) << endl;
+
+  cout << "revert cos = " << cos(angle) << endl;
+  cout << "revert sin = " << sin(angle) << endl;
+  cout << "full cycle sin: " << sin(asin(sin_value)) << endl;
 
   return angle;
 }
 
 DPoint convert_abs_coord(double const angle, double const ratio)
 {
-  return DPoint(cos(angle) / ratio, - (sin(angle) / ratio));
+  return DPoint(cos(angle) / ratio, sin(angle) / ratio);
 }
 
-Point convert_img_coord(Image const& img, DPoint const& p)
+APoint convert_img_coord(Image const& img, DPoint const& p)
 {
   int x = round(p.x + (img.width / 2.0f) - 0.5);
   int y = round(p.y + (img.height / 2.0f) - 0.5);
-  Point p_mapped(x, y);
+  APoint p_mapped(x, y);
 //  if (p_mapped.x >= img.width || p_mapped.y >= img.height)
 //  {
 //    cerr << "Point coord mapping" << endl;
@@ -263,10 +298,7 @@ Point convert_img_coord(Image const& img, DPoint const& p)
 
 double compute_ratio(Image const& img)
 {
-  unsigned int const nb_points = img.width * img.height;
-  double const square_side = sqrt(nb_points) - 1;
-  double const half_side = square_side / 2;
-  double const trigo_length = ceil(half_side * sqrt(2));
+  double const trigo_length = (sqrt(img.width * img.width + img.height * img.height) - 1) / 2;
   return 1.0f / trigo_length;
 }
 
@@ -292,7 +324,11 @@ void compute_output_size(Image const& src, double const rotation, unsigned int&
   max_w = max(max_w, tl.x);
   min_h = min(min_h, tl.y);
   max_h = max(max_h, tl.y);
-  //cout << "Rotated " << p << " (" << angle << ") = " << tl << "(" << angle + rotation << ")" << endl << endl;
+  // debug print
+  if (rotation == 0.0)
+  {
+    cout << "Rotated " << p << "  = " << tl << endl << endl;
+  }
 
   p = Point(src.width - 1, 0);
   angle = convert_radian(src, p, ratio);
@@ -301,7 +337,10 @@ void compute_output_size(Image const& src, double const rotation, unsigned int&
   max_w = max(max_w, tr.x);
   min_h = min(min_h, tr.y);
   max_h = max(max_h, tr.y);
-  //cout << "Rotated " << p << " (" << angle << ") = " << tr << "(" << angle + rotation << ")" << endl << endl;
+  if (rotation == 0.0)
+  {
+    cout << "Rotated " << p << "  = " << tr << endl << endl;
+  }
 
   p = Point(0, src.height - 1);
   angle = convert_radian(src, p, ratio);
@@ -310,7 +349,10 @@ void compute_output_size(Image const& src, double const rotation, unsigned int&
   max_w = max(max_w, bl.x);
   min_h = min(min_h, bl.y);
   max_h = max(max_h, bl.y);
-  //cout << "Rotated " << p << " (" << angle << ") = " << bl << "(" << angle + rotation << ")" << endl << endl;
+  if (rotation == 0.0)
+  {
+    cout << "Rotated " << p << "  = " << bl << endl << endl;
+  }
 
   p = Point(src.width - 1, src.height - 1);
   angle = convert_radian(src, p, ratio);
@@ -319,35 +361,44 @@ void compute_output_size(Image const& src, double const rotation, unsigned int&
   max_w = max(max_w, br.x);
   min_h = min(min_h, br.y);
   max_h = max(max_h, br.y);
-  //cout << "Rotated " << p << " (" << angle << ") = " << br << "(" << angle + rotation << ")" << endl << endl;
+  if (rotation == 0.0)
+  {
+    cout << "Rotated " << p << "  = " << br << endl << endl;
+  }
 
   width = (int) (max_w - min_w) + 1;
   height = (int) (max_h - min_h) + 1;
 }
 
+void convert_abs_to_polar_coord(DPoint const& p, double const ratio, double& angle, double& dist)
+{
+  double const cos_value = p.x * ratio;
+  double const sin_value = - (p.y * ratio);
+  angle = acos(cos_value);
+  if (sin_value < 0)
+    angle = 2 * M_PI - angle;
+
+  dist = sqrt(p.x * p.x + p.y * p.y);
+}
+
+DPoint convert_polar_to_grid_coord(double const angle, double const distance)
+{
+  return DPoint(cos(angle) * distance, - (sin(angle) * distance));
+}
+
 
 //
 //
-// Rotation
+// Point rotation
 //
 
-Point rotate(Image const& src, Point const& p, double const ratio, double const rotation, Image const& rotated)
+APoint rotate(Image const& src, Point const& p, double const ratio, double const rotation, Image const& rotated)
 {
   double angle = convert_radian(src, p, ratio);
   DPoint a_point = convert_abs_coord(angle + rotation, ratio);
   return convert_img_coord(rotated, a_point);
 }
 
-Image rotate(Image const& src, double angle)
-{
-  // FIXME
-  Image rot(0, 0);
-  cout << angle << src.width << endl; // to remove warnings
-  return rot;
-}
-
-
-
 //
 //
 // Drawing
@@ -390,7 +441,7 @@ void draw_line(Image& img, unsigned int x1, unsigned int y1, unsigned int x2, un
   }
 }
 
-void draw_line(Image& img, Point const& p1, Point const& p2)
+void draw_line(Image& img, APoint const& p1, APoint const& p2)
 {
   draw_line(img, p1.x, p1.y, p2.x, p2.y);
 }
@@ -405,10 +456,10 @@ void draw_outline(Image const& input, unsigned int degrees, string const& name)
   Image rotated(w, h);
 
   double const ratio = compute_ratio(input);
-  Point tl = rotate(input, Point(0, 0), ratio, rotation, rotated);
-  Point tr = rotate(input, Point(input.width - 1, 0), ratio, rotation, rotated);
-  Point bl = rotate(input, Point(0, input.height - 1), ratio, rotation, rotated);
-  Point br = rotate(input, Point(input.width - 1, input.height - 1), ratio, rotation, rotated);
+  APoint tl = rotate(input, Point(0, 0), ratio, rotation, rotated);
+  APoint tr = rotate(input, Point(input.width - 1, 0), ratio, rotation, rotated);
+  APoint bl = rotate(input, Point(0, input.height - 1), ratio, rotation, rotated);
+  APoint br = rotate(input, Point(input.width - 1, input.height - 1), ratio, rotation, rotated);
   cout << tl << " " << tr << " " << bl << " " << br << endl;
 
   draw_line(rotated, tl, tr);
@@ -422,6 +473,55 @@ void draw_outline(Image const& input, unsigned int degrees, string const& name)
 }
 
 
+
+//
+//
+// Image rotation
+//
+
+Image rotate(Image const& src, double angle)
+{
+  double const rotation = (angle / 180.0f) * M_PI;
+  unsigned int w = 0;
+  unsigned int h = 0;
+  compute_output_size(src, rotation, w, h);
+  Image rotated(w, h);
+
+  // debug print
+  if (rotation == 0.0)
+  {
+    cout << "src dimensions: " << src.width << " x " << src.height << endl;
+    cout << "rotated dimensions: " << w << " x " << h << endl;
+  }
+
+  double const ratio = compute_ratio(src);
+
+  for (int y = 0; y < (int) rotated.height; ++y)
+  {
+    for (int x = 0; x < (int) rotated.width; ++x)
+    {
+      Point const p(x, y);
+      DPoint const d = convert_grid_coord(rotated, p);
+      double p_angle = 0;
+      double dist = 0;
+      convert_abs_to_polar_coord(d, ratio, p_angle, dist);
+      DPoint const src_rotated_point = convert_polar_to_grid_coord(p_angle - rotation, dist);
+
+      // FIXME: get source points
+      APoint src_p = convert_img_coord(src, src_rotated_point);
+      uint8_t r = 0;
+      uint8_t g = 0;
+      uint8_t b = 0;
+      src.get_pixel(src_p, r, g, b);
+      rotated.set_pixel(p, r, g, b);
+    }
+  }
+
+  return rotated;
+}
+
+
+
 //
 //
 // Check
@@ -457,7 +557,7 @@ bool check_trigo()
   // Check that we can reverse the origin point.
   DPoint const abs_reverse_point = convert_abs_coord(angle, ratio);
   cout << "reversed abs origin: " << abs_reverse_point << endl;
-  Point const reverse_point = convert_img_coord(square, abs_reverse_point);
+  APoint const reverse_point = convert_img_coord(square, abs_reverse_point);
   cout << "reversed origin in square: " << reverse_point << endl;
   if (abs(0.0 - reverse_point.x) > sigma)
   {
@@ -493,7 +593,7 @@ bool check_trigo()
     Image rotated(w, h);
 
     DPoint const a_p45 = convert_abs_coord(angle + rotation, ratio);
-    Point const p45 = convert_img_coord(rotated, a_p45);
+    APoint const p45 = convert_img_coord(rotated, a_p45);
     if (!fequal(0, p45.x, sigma))
     {
       cerr << __LINE__ << " > Rotation origin by 45 degrees:" << endl;
@@ -543,19 +643,35 @@ void check_lines()
 
 int main()
 {
-  check_points();
+  bool perform_check = false;
 
-  if (!check_trigo())
-    return 1;
+  if (perform_check)
+  {
+    if (!check_points())
+      return 1;
 
-  check_lines();
+    if (!check_trigo())
+      return 1;
 
-  return 0;
+    check_lines();
+  }
 
   Image img("img/luigi.ppm");
 
-  Image rotated = rotate(img, 30);
-  rotated.save("rotated.ppm");
+  for (double rotation : {0, 15, 30})
+  {
+    auto const before = chrono::high_resolution_clock::now();
+
+    Image rotated = rotate(img, rotation);
+
+    auto const after = chrono::high_resolution_clock::now();
+    auto const duration_ms = std::chrono::duration_cast<std::chrono::milliseconds>(after - before);
+    cout << "rotate(): " << duration_ms.count() << " ms" << endl;
+
+    stringstream filename;
+    filename << "rotated_" << rotation << ".ppm";
+    rotated.save(filename.str());
+  }
 
   return 0;
 }