#include <string>
#include <fstream>
#include <iostream>
#include <sstream>
#include <cmath>
#include <cassert>
#include <cstring>
#include <chrono>

#include <xmmintrin.h>


using namespace std;


template <typename T>
struct TPoint {
  T x;
  T y;

  TPoint(T a, T b)
  : x(a)
  , y(b)
  {}
};
typedef TPoint<int> Point;
typedef TPoint<double> DPoint; // absolute point, can be negative
template<typename Elem, typename Traits, typename T>
std::basic_ostream<Elem, Traits>& operator << (std::basic_ostream<Elem, Traits>& o, TPoint<T> const& p)
{
  o << "(" << p.x << ", " << p.y << ")";
  return o;
}

struct Image {
  unsigned int width;
  unsigned int height;
  uint8_t* buffer;

  Image()
  : width(0)
  , height(0)
  , buffer(NULL)
  {}

  Image(unsigned int w, unsigned int h)
  {
    this->width = w;
    this->height = h;
    buffer = new uint8_t[width * height * 3];
    memset(buffer, 0, width * height * 3 * sizeof (uint8_t));
  }

  Image(string const& path)
  : Image()
  {
    ifstream is(path);
    if (!is.is_open())
    {
      cerr << "Cannot open file '" << path << "'" << endl;
      abort();
    }

    if (!this->read_header(is))
    {
      cerr << "Invalid header." << endl;
      abort();
    }

    if (!this->read_body(is))
    {
      delete buffer;
      buffer = nullptr;

      cerr << "Invalid header." << endl;
      abort();
    }
  }

  bool save(string const& path)
  {
    ofstream os(path);
    if (!os.is_open())
    {
      cerr << "Cannot open file '" << path << "'" << endl;
      return false;
    }
    this->write_header(os);
    this->write_body(os);
    return true;
  }

  void set_pixel(unsigned int x, unsigned int y, uint8_t r, uint8_t g, uint8_t b)
  {
    if (x >= width || y >= height)
    {
//        cerr << __LINE__ << " | Point (" << x << ", " << y << ") out of bounds" << endl;
//        cerr << " Image dimensions: " << width << " x " << height << endl;
//        assert(false);
        return;
    }
    int index = (y * width + x) * 3;
    buffer[index++] = r;
    buffer[index++] = g;
    buffer[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);
  }


  private:
    bool read_header(std::ifstream& istr)
    {
      // check magic
      if (istr.get() != 'P' )
      {
        return false;
      }

      char type = static_cast<char>(istr.get());
      if (type != '6')
      {
        return false;
      }

      if (istr.get() != '\n')
      {
        return false;
      }

      // skip comments
      while (istr.peek() == '#')
      {
        std::string line;
        std::getline(istr, line);
      }

      // get size
      istr >> width >> height;
      if (width == 0 || height == 0)
      {
        return false;
      }

      // get maxvalue
      if (istr.get() != '\n')
      {
        return false;
      }

      int max_value = -1;
      istr >> max_value;
      if (max_value > 255)
      {
        return false;
      }

      if (istr.get() != '\n')
      {
        return false;
      }

      // cout << "width: " << width << endl;
      // cout << "height: " << height << endl;

      return true;
    }

    bool write_header(std::ofstream& ostr)
    {
      ostr << "P6" << endl;
      ostr << width << " " << height << endl;
      ostr << "255" << endl;
      return true;
    }

    bool read_body(std::ifstream& istr)
    {
      unsigned int const nb_pixels = width * height;
      buffer = new uint8_t[nb_pixels * 3];

      uint8_t* buf_index = buffer;
      for (unsigned int i = 0; i < nb_pixels * 3; ++i)
      {
        *buf_index = istr.get();
        ++buf_index;
      }

      return true;
    }

    bool write_body(std::ofstream& ostr)
    {
      unsigned int const nb_pixels = width * height;
      uint8_t* buf_index = buffer;
      for (unsigned int i = 0; i < nb_pixels * 3; ++i)
      {
        ostr << (char) *buf_index;
        ++buf_index;
      }

      return true;
    }
};



//
//
// Trigonometry
//

DPoint convert_grid_coord(Image const& img, Point const& p)
{
  return DPoint(p.x - img.width / 2.0f + 0.5, p.y - img.height / 2.0f + 0.5);
}

double convert_radian(Image const& img, Point const& p, double const ratio)
{
  DPoint centered = convert_grid_coord(img, p);
  double const cos_value = centered.x * ratio;
  double const sin_value = - (centered.y * ratio);
  double angle = acos(cos_value);
  if (sin_value < 0)
  {
    angle = (2 * M_PI) - angle;
  }

  return angle;
}

DPoint convert_abs_coord(double const angle, double const ratio)
{
  return DPoint(cos(angle) / ratio, - sin(angle) / ratio);
}

Point 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);
  return Point(x, y);
}

DPoint convert_img_coord_precision(Image const& img, DPoint const& p)
{
  int x = p.x + (img.width / 2.0f) - 0.5;
  int y = p.y + (img.height / 2.0f) - 0.5;
  return DPoint(x, y);
}

void convert_abs_to_polar_coord(DPoint const& p, double& angle, double& dist)
{
  angle = atan2(-p.y, p.x);
  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));
}

double compute_ratio(Image const& img)
{
  double const trigo_length = (sqrt(img.width * img.width + img.height * img.height) - 1) / 2;
  return 1.0f / trigo_length;
}

inline
bool fequal(float a, float b, float sigma)
{
  return abs(a - b) < sigma;
}

void compute_output_size(Image const& src, double const rotation, unsigned int& width, unsigned int& height)
{
  double const ratio = compute_ratio(src);
  double min_w = 0;
  double max_w = 0;
  double min_h = 0;
  double max_h = 0;

  Point p(0, 0);
  double angle = convert_radian(src, p, ratio);
  DPoint tl = convert_abs_coord(angle + rotation, ratio);
  min_w = min(min_w, tl.x);
  max_w = max(max_w, tl.x);
  min_h = min(min_h, tl.y);
  max_h = max(max_h, tl.y);

  p = Point(src.width - 1, 0);
  angle = convert_radian(src, p, ratio);
  DPoint tr = convert_abs_coord(angle + rotation, ratio);
  min_w = min(min_w, tr.x);
  max_w = max(max_w, tr.x);
  min_h = min(min_h, tr.y);
  max_h = max(max_h, tr.y);

  p = Point(0, src.height - 1);
  angle = convert_radian(src, p, ratio);
  DPoint bl = convert_abs_coord(angle + rotation, ratio);
  min_w = min(min_w, bl.x);
  max_w = max(max_w, bl.x);
  min_h = min(min_h, bl.y);
  max_h = max(max_h, bl.y);

  p = Point(src.width - 1, src.height - 1);
  angle = convert_radian(src, p, ratio);
  DPoint br = convert_abs_coord(angle + rotation, ratio);
  min_w = min(min_w, br.x);
  max_w = max(max_w, br.x);
  min_h = min(min_h, br.y);
  max_h = max(max_h, br.y);

  width = (int) (max_w - min_w) + 1;
  height = (int) (max_h - min_h) + 1;
}



//
//
// Point rotation
//

Point 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);
}



//
//
// Drawing
//

void draw_line(Image& img, unsigned int x1, unsigned int y1, unsigned int x2, unsigned int y2)
{
  int x_inc = x1 <= x2 ? 1 : -1;

  unsigned int const y_min = min(y1, y2);
  unsigned int const y_max = max(y1, y2);
  double slope = (double) y2 - y1;
  if (x1 != x2)
    slope = ((double) y2 - y1) / abs((double) x2 - x1);
  int y_inc = slope > 0 ? 1 : -1;

  if (x1 == x2)
  {
    for (unsigned int runner = y1; runner != y2; runner+= y_inc)
      img.set_pixel(x1, runner, 255, 0, 0); // set line to red
    return;
  }

  if (y1 == y2)
  {
    for (unsigned int runner = x1; runner != x2; runner+= x_inc)
      img.set_pixel(runner, y1, 255, 0, 0); // set line to red
    return;
  }

  unsigned int previous_y = y1;
  for (unsigned int i = x1, steps = 0; i != x2; i += x_inc, ++steps)
  {
    unsigned int y = slope * steps + y1;
    y = min(y, y_max);
    y = max(y, y_min);
    for (unsigned int runner = previous_y; runner != y; runner+= y_inc)
      img.set_pixel(i, runner, 255, 0, 0); // set line to red
    previous_y = y;
  }
}

void draw_line(Image& img, Point const& p1, Point const& p2)
{
  draw_line(img, p1.x, p1.y, p2.x, p2.y);
}

void draw_outline(Image const& input, unsigned int degrees, string const& name)
{
  double const rotation = (degrees / 180.0f) * M_PI;
  unsigned int w = 0;
  unsigned int h = 0;
  compute_output_size(input, rotation, w, h);
  cout << "rotation(" << degrees << ") -> " << w << " x " << h << endl;
  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);
  cout << tl << " " << tr << " " << bl << " " << br << endl;

  draw_line(rotated, tl, tr);
  draw_line(rotated, tr, br);
  draw_line(rotated, br, bl);
  draw_line(rotated, bl, tl);

  stringstream ss;
  ss << "check_lines_" << name << "_" << degrees << ".ppm";
  rotated.save(ss.str());
}



//
//
// Math approximation
//

void round_if_very_small(double& d)
{
  if (abs(d) < 1.0e-10)
    d = 0.0;
}


//
//
// Image rotation
//

DPoint get_mapped_point(Image const& src, Point const& p, double const rotation)
{
  DPoint const d = convert_grid_coord(src, p);
  double p_angle = 0;
  double dist = 0;
  convert_abs_to_polar_coord(d, p_angle, dist);
  return convert_polar_to_grid_coord(p_angle + rotation, dist);
}

inline
void rotate_pixel(Image const& src, Image& rotated,
                  DPoint const& src_rotated_point, Point const& rot_point,
                  unsigned int const src_limit, unsigned int const rot_limit)
{
  unsigned int src_index = ((int) src_rotated_point.y * src.width + (int) src_rotated_point.x) * 3;
  unsigned int rot_index = (rot_point.y * rotated.width + rot_point.x) * 3;

  // Out-of-bounds check
  if (src_index >= src_limit
      || rot_index >= rot_limit)
    return;

  // Bilinear interpolation
  unsigned int src_index_1 = src_index;
  unsigned int src_index_2 = src_index_1 + 3;
  unsigned int src_index_3 = src_index_1 + 3 * src.width;
  unsigned int src_index_4 = src_index_3 + 3;

  if (src_index_4 >= src_limit)
    return;

  double x_delta = src_rotated_point.x - floor(src_rotated_point.x);
  round_if_very_small(x_delta);
  double y_delta = src_rotated_point.y - floor(src_rotated_point.y);
  round_if_very_small(y_delta);

  // SIMD
  __m128 const x_d = _mm_set_ps1(x_delta);
  __m128 const inv_x_d = _mm_set_ps1(1 - x_delta);
  __m128 top_left = _mm_set_ps(src.buffer[src_index_1], src.buffer[src_index_1 + 1], src.buffer[src_index_1 + 2], 0.0);
  __m128 top_right = _mm_set_ps(src.buffer[src_index_2], src.buffer[src_index_2 + 1], src.buffer[src_index_2 + 2], 0.0);
  top_left = _mm_mul_ps(top_left, inv_x_d);
  top_right = _mm_mul_ps(top_right, x_d);
  top_left = _mm_add_ps(top_left, top_right);

  __m128 bottom_left = _mm_set_ps(src.buffer[src_index_3], src.buffer[src_index_3 + 1], src.buffer[src_index_3 + 2], 0.0);
  __m128 bottom_right = _mm_set_ps(src.buffer[src_index_4], src.buffer[src_index_4 + 1], src.buffer[src_index_4 + 2], 0.0);
  bottom_left = _mm_mul_ps(bottom_left, inv_x_d);
  bottom_right = _mm_mul_ps(bottom_right, x_d);
  bottom_left = _mm_add_ps(bottom_left, bottom_right);

  __m128 const y_d = _mm_set_ps1(y_delta);
  __m128 const inv_y_d = _mm_set_ps1(1 - y_delta);
  top_left = _mm_mul_ps(top_left, inv_y_d);
  bottom_left = _mm_mul_ps(bottom_left, y_d);
  top_left = _mm_add_ps(top_left, bottom_left);

  // convert float values to uint8_t
  rotated.buffer[rot_index] = top_left[3];
  rotated.buffer[rot_index + 1] = top_left[2];
  rotated.buffer[rot_index + 2] = top_left[1];
}

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);

  // corner points in rotated image
  // TODO: add one ligne for smooth border
  DPoint const tl_grid = get_mapped_point(src, Point(0, 0), rotation);
  Point const tl = convert_img_coord(rotated, tl_grid);
  DPoint const tr_grid = get_mapped_point(src, Point(src.width - 1, 0), rotation);
  Point const tr = convert_img_coord(rotated, tr_grid);
  DPoint const bl_grid = get_mapped_point(src, Point(0, src.height - 1), rotation);
  Point const bl = convert_img_coord(rotated, bl_grid);

  // corner points in source image
  DPoint src_tl = get_mapped_point(rotated, tl, -rotation);
  src_tl = convert_img_coord_precision(src, src_tl);

  DPoint src_origin = get_mapped_point(rotated, Point(0, 0), -rotation);
  DPoint src_delta_x = get_mapped_point(rotated, Point(1, 0), -rotation);
  DPoint src_delta_y = get_mapped_point(rotated, Point(0, 1), -rotation);

  src_delta_x.x = src_delta_x.x - src_origin.x;
  src_delta_x.y = src_delta_x.y - src_origin.y;
  round_if_very_small(src_delta_x.x);
  round_if_very_small(src_delta_x.y);
  src_delta_y.x = src_delta_y.x - src_origin.x;
  src_delta_y.y = src_delta_y.y - src_origin.y;
  round_if_very_small(src_delta_y.x);
  round_if_very_small(src_delta_y.y);
  // cout << "src delta x = " << src_delta_x << endl;
  // cout << "src delta y = " << src_delta_y << endl;


  // // steps for first column in source image (y)
  int origin_nb_steps = max(abs(bl.x - tl.x), abs(bl.y - tl.y));
  // // steps for line in source image (x)
  int line_nb_steps = max(abs(tr.x - tl.x), abs(tr.y - tl.y));

  // steps for first column in rotated image (y)
  DPoint rotated_step((bl.x - tl.x) / (float) origin_nb_steps, (bl.y - tl.y) / (float) origin_nb_steps);

  // steps for line in rotated image (x)
  DPoint bresenham((tr.x - tl.x) / (float) line_nb_steps, (tr.y - tl.y) / (float) line_nb_steps);

  unsigned int const src_limit = src.width * src.height * 3;
  unsigned int const rot_limit = rotated.width * rotated.height * 3;

  for (int y_i = 0; y_i <= (int) origin_nb_steps; ++y_i)
  {
    // first column origin
    Point const rot_origin(tl.x + y_i * rotated_step.x, tl.y + y_i * rotated_step.y);

    Point previous = rot_origin;

    for (int x_i = 0; x_i <= (int) line_nb_steps; ++x_i)
    {
      Point rot_point(rot_origin.x + x_i * bresenham.x, rot_origin.y + x_i * bresenham.y);

      Point const delta(rot_point.x - tl.x, rot_point.y - tl.y);
      DPoint src_rotated_point(src_tl.x + delta.x * src_delta_x.x + delta.y * src_delta_y.x,
                               src_tl.y + delta.x * src_delta_x.y + delta.y * src_delta_y.y);

      rotate_pixel(src, rotated, src_rotated_point, rot_point, src_limit, rot_limit);

      if (previous.x != rot_point.x && previous.y != rot_point.y)
      {
        int y_slope = rot_point.y > previous.y ? 1 : -1;
        int tmp_y = rot_point.y;
        rot_point.y = previous.y;

        src_rotated_point.x -= y_slope * src_delta_y.x;
        src_rotated_point.y -= y_slope * src_delta_y.y;

        rotate_pixel(src, rotated, src_rotated_point, rot_point, src_limit, rot_limit);

        rot_point.y = tmp_y;
      }

      previous = rot_point;
    }
  }

  return rotated;
}



//
//
// Check
//

bool check_points()
{
  Image five(5, 5);
  Point origin(0, 0);
  DPoint d1 = convert_grid_coord(five, origin);
  assert(d1.x == -2);
  assert(d1.y == -2);

  return true;
}

bool check_trigo()
{
  Image square(500, 500);
  double const ratio = compute_ratio(square);
  double const sigma = 1.0e-2;
  if (!fequal(ratio, 1 / 707.106, sigma))
  {
    cerr << __LINE__ << " | Invalid ratio: " << ratio << " != " << 1 / 707.106 << endl;
    return false;
  }

  // Check that the origin of a square image is at sqrt(2) / 2
  double const angle = convert_radian(square, Point(0, 0), ratio);

  if (!fequal(angle, 3 * M_PI / 4, sigma))
  {
    cerr << __LINE__ << " | Invalid angle value: " << angle << " != " << 3 * M_PI / 4 << endl;
    return false;
  }

  // Check that we can reverse the origin point.
  DPoint const abs_reverse_point = convert_abs_coord(angle, ratio);
  Point const reverse_point = convert_img_coord(square, abs_reverse_point);
  if (!fequal(0.0, reverse_point.x, sigma)
    || !fequal(0.0, reverse_point.y, sigma))
  {
    cerr << __LINE__ << "Reverse origin fail" << endl;
    cerr << " " << reverse_point << " != (0, 0)" << endl;
    cerr << " abs point " << abs_reverse_point << endl;
    return false;
  }

  // Check that when rotating the origin by 45 degrees
  double const rotation = M_PI / 4; // 45 degrees
  unsigned int w = 0;
  unsigned int h = 0;
  compute_output_size(square, rotation, w, h);

  if (!fequal(w, square.width * sqrt(2), sigma * square.width)
      || !fequal(h, square.height * sqrt(2), sigma * square.height))
  {
    cerr << "Invalid rotated image dimensions " << w << " x " << h << endl;
    cerr << " expected " << (int) ceil(square.width * sqrt(2)) << " x " << (int) ceil(square.height * sqrt(2)) << endl;
    return false;
  }


  Image rotated(w, h);

  DPoint const a_p45 = convert_abs_coord(angle + rotation, ratio);
  Point const p45 = convert_img_coord(rotated, a_p45);
  if (!fequal(0, p45.x, sigma))
  {
    cerr << __LINE__ << " > Rotation origin by 45 degrees:" << endl;
    cerr << " invalid x value: " << p45.x << " != " << 0 << endl;
    cerr << " absolute point: " << a_p45 << endl;
    cerr << " relative point: " << p45 << endl;
    return false;
  }
  if (!fequal(p45.y, (h - 1) / 2.0f, sigma))
  {
    cerr << __LINE__ << " > Rotation origin by 45 degrees:" << endl;
    cerr << "Invalid y value: " << p45.y << " != " << (h - 1) / 2.0f << endl;
    cerr << " absolute point: " << a_p45 << endl;
    cerr << " relative point: " << p45 << endl;
    return false;
  }

  // Polar coordinates
  {
    DPoint const d(-42.5, 37.5);
    double angle = 0;
    double dist = 0;
    convert_abs_to_polar_coord(d, angle, dist);
    DPoint const reversed = convert_polar_to_grid_coord(angle, dist);
    if (!fequal(d.x, reversed.x, sigma)
        || !fequal(d.y, reversed.y, sigma))
    {
      cerr << __LINE__ << " > Reverse polar coordinates:" << endl;
      cerr << reversed << " != " << d << endl;
      cerr << "polar (" << angle << ", " << dist << ")" << endl;
      return false;
    }
  }

  return true;
}

bool check_90(string const& path)
{
  Image const src(path);
  Image const rotated = rotate(src, 90);

  for (unsigned int y = 0; y < rotated.height; ++y)
  {
    for (unsigned int x = 0; x < rotated.width; ++x)
    {
      unsigned rot_index = (y * rotated.width + x) * 3;
      unsigned src_index = (x * src.width + (src.width - 1 - y)) * 3;
      if (memcmp(&rotated.buffer[rot_index], &src.buffer[src_index], 3 * sizeof (uint8_t)) != 0)
      {
        Point r(x, y);
        Point s((src.width - 1 - y), x);
        cerr << __LINE__ << " | R: " << r << " != S:" << s << endl;
        cerr << "R dim: " << rotated.width << " x " << rotated.height << endl;
        cerr << "S dim: " << src.width << " x " << src.height << endl;
        return false;
      }
    }
  }

  return true;
}



//
//
// Main
//

int main(int argc, char* argv[])
{
  if (argc < 2)
  {
    cout << "Usage: " << argv[0] << " image.ppm" << endl;
    return 1;
  }

  bool perform_check = true;

  if (perform_check)
  {
    if (!check_points())
      return 1;

    if (!check_trigo())
      return 1;

    if (!check_90(argv[1]))
    {
      cerr << __LINE__ << " | 90 degrees check failed" << endl;
//      return 1;
    }
  }

  Image img(argv[1]);

  for (double rotation = 0; rotation < 360; rotation += 5)
  {
  //  double rotation = 45;
    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(" << rotation << "): " << duration_ms.count() << " ms" << endl;

    stringstream filename;
    // filename << "/tmp/";
    filename << "rotated_";
    if (rotation < 100)
      filename << "0";
    if (rotation < 10)
      filename << "0";
    filename << rotation << ".ppm";
    rotated.save(filename.str());
  }

  return 0;
}