/*
  Coord : 座標定義用構造体
*/
template< class T >
struct Coord
{
  T x; // x 座標
  T y; // y 座標

  /* コンストラクタ */

  // デフォルト・コンストラクタ
  // ( x, y ) = ( 0, 0 ) で初期化される
  Coord() : x( 0 ), y ( 0 ) {}

  // ( x, y ) 座標を指定して構築
  Coord( T x_, T y_ ) : x( x_ ), y ( y_ ) {}

  // 異なる型の座標から構築
  template< class U >
  Coord( const Coord< U >& c ) : x( c.x ), y( c.y ) {}
};

/*
  Matrix : 行列定義用クラス
*/
template< class T >
class Matrix
{
  // 要素を保持するコンテナの型
  typedef typename std::vector< T > array_type;

  // 行・列のサイズ
  Coord< typename array_type::size_type > size_;
  // 行列要素を保持するコンテナ
  array_type d_;

public:

  // 行列のサイズを表す型
  typedef typename array_type::size_type size_type;
  // 行列の要素の型
  typedef T value_type;

  // デフォルト・コンストラクタ
  Matrix() {}

  // コンストラクタ
  // 行列のサイズを指定して構築
  explicit Matrix( Coord< size_type > size, value_type val = value_type() )
    : size_( size ), d_( size.x * size.y, val ) {}

  // 指定したサイズ size と値 val で初期化する
  void assign( Coord< size_type > size, value_type val = value_type() )
  {
    size_ = size;
    d_.assign( size.x * size.y, val );
  }

  // 座標 p の要素への参照を返す
  T& operator[]( Coord< int > p )
  {
    Coord< int > sz( size() );
    if ( p.x < 0 || p.y < 0 || p.x >= sz.x || p.y >= sz.y )
      throw std::domain_error( "Matrix::operator[] : Invalid coord [p]." );
    return( d_[ p.y * xSize() + p.x ] );
  }

  // 座標 p の要素を返す
  T operator[]( Coord< int > p ) const
  {
    Coord< int > sz( size() );
    if ( p.x < 0 || p.y < 0 || p.x >= sz.x || p.y >= sz.y )
      throw std::domain_error( "Matrix::operator[] : Invalid coord [p]." );
    return( d_[ p.y * xSize() + p.x ] );
  }

  // 座標 ( x, y ) の要素への参照を返す
  T& operator()( int x, int y )
  {
    Coord< int > sz( size() );
    if ( x < 0 || y < 0 || x >= sz.x || y >= sz.y )
      throw std::domain_error( "Matrix::operator() : Invalid coord [x,y]." );
    return( d_[ y * xSize() + x ] );
  }

  // 座標 ( x, y ) の要素を返す
  T operator()( int x, int y ) const
  {
    Coord< int > sz( size() );
    if ( x < 0 || y < 0 || x >= sz.x || y >= sz.y )
      throw std::domain_error( "Matrix::operator() : Invalid coord [x,y]." );
    return( d_[ y * xSize() + x ] );
  }

  // 行列のサイズを返す
  Coord< size_type > size() const
  { return( size_ ); }

  // 列数 ( X 方向のサイズ ) を返す
  size_type xSize() const
  { return( size_.x ); }

  // 行数 ( Y 方向のサイズ ) を返す
  size_type ySize() const
  { return( size_.y ); }
};

/*
  SquareMatrix : 正方行列定義用クラス
*/
template< class T >
class SquareMatrix
{
  // 行列の要素を保持するコンテナ ( Matrix クラス )
  Matrix< T > mat_;

public:

  // 行列のサイズを表す型
  typedef typename Matrix<T>::size_type size_type;
  // 行列の要素を表す型
  typedef T value_type;

  // コンストラクタ
  // 行列のサイズを指定して構築
  explicit SquareMatrix( size_type size, value_type val = value_type() )
    : mat_( Coord< size_type >( size, size ), val ) {}

  // 座標 p の要素への参照を返す
  T& operator[]( Coord< int > p )
  { return( mat_[p] ); }

  // 座標 p の要素を返す
  T operator[]( Coord< int > p ) const
  { return( mat_[p] ); }

  // 座標 ( x, y ) の要素への参照を返す
  T& operator()( int x, int y )
  { return( mat_( x, y ) ); }

  // 座標 ( x, y ) の要素を返す
  T operator()( int x, int y ) const
  { return( mat_( x, y ) ); }

  // 行列のサイズを返す
  size_type size() const
  { return( mat_.xSize() ); }
};

/*
  CalcCoeff : 離散コサイン変換(DCT)用の係数を計算して coeff に登録する

  C[u,x] = cos( πu( 2x + 1 ) / 2N )

  以下のような行列が作成される
  | 1/√2 cos π/2N       ... cos (N-1)π/2N       |
  | 1/√2 cos 3π/2N      ... cos (N-1)3π/2N      |
  |   :        :                      :          |
  | 1/√2 cos (2N-1)π/2N ... cos (N-1)(2N-1)π/2N |

  N : 係数行列のサイズ
*/
void CalcCoeff( SquareMatrix< double >* coeff )
{
  int mcuSize = coeff->size(); // MCU のサイズ

  for ( int x = 0 ; x < mcuSize ; ++x )
    for ( int u = 0 ; u < mcuSize ; ++u )
      (*coeff)( u, x )= cos( ( M_PI * u * ( 2 * x + 1 ) ) / ( 2.0 * mcuSize ) );
}

/*
  GetMCUCount : MCU の X,Y 方向の数を求める

  gSize は画像サイズ、mcuSize は MCU のサイズ、skip は間引き量をそれぞれ表す
*/
Coord< int > GetMCUCount( Coord< int > gSize, int mcuSize, Coord< int > skip )
{
  Coord< int > mcuCount; // 求める値

  mcuCount.x = gSize.x / ( mcuSize * skip.x );
  if ( gSize.x % ( mcuSize * skip.x ) != 0 )
    ++( mcuCount.x );
  mcuCount.y = gSize.y / ( mcuSize * skip.y );
  if ( gSize.y % ( mcuSize * skip.y ) != 0 )
    ++( mcuCount.y );

  return( mcuCount );
}

/*
  dct_x : X 成分に対する離散コサイン変換(DCT)

  yuv は YUVデータの一成分からなる二次元配列を表し、係数行列 coeff を元に DCT 計算をして
  結果を f_uy に返す。mcuCoord は対象となる MCU の位置、skip は間引き量をそれぞれ表す。
*/
void dct_x( const Matrix< double >& yuv, const SquareMatrix< double >& coeff,
            SquareMatrix< double >* f_uy, Coord< int > mcuCoord, Coord< int > skip )
{
  // 画像 ( YUV データ ) のサイズ
  Coord< int > gSize = yuv.size();
  // MCU のサイズ
  int mcuSize = f_uy->size();

  // YUV データ上の対象座標
  Coord< int > c_yuv( 0, mcuCoord.y * mcuSize * skip.y );

  int y = 0;
  for ( ; y < mcuSize ; ++y ) {
    if ( c_yuv.y >= gSize.y ) break;
    for ( int u = 0 ; u < mcuSize ; u++ ) {
      double a = 0; // 求める係数
      c_yuv.x = mcuCoord.x * mcuSize * skip.x;
      // y 行について、X 方向の YUV 成分と u 列目の係数ベクトルの内積を求める
      for ( int x = 0 ; x < mcuSize ; ++x ) {
        if ( c_yuv.x >= gSize.x ) break;
        a += yuv[c_yuv] * coeff( u, x );
        c_yuv.x += skip.x;
      }
      if ( u == 0 ) a /= sqrt( 2 );
      (*f_uy)( u, y ) = a;
    }
    c_yuv.y += skip.y;
  }

  // 残りの行はゼロクリア
  for ( ; y < mcuSize ; ++y )
    for ( int u = 0 ; u < mcuSize ; u++ )
      (*f_uy)( u, y ) = 0;
}

/*
  dct_y : Y 成分に対する離散コサイン変換(DCT)

  f_uy は X 方向の成分で計算した結果を表し、係数行列 coeff を元に DCT 計算をして結果を mcu に返す。
*/
void dct_y( const SquareMatrix< double >& f_uy, const SquareMatrix< double >& coeff,
            Coord< int > mcuCoord, Matrix< double >* mcu )
{
  // MCU のサイズ
  int mcuSize = f_uy.size();

  // mcu への書き込み開始位置
  Coord< int > sp( mcuCoord.x * mcuSize, mcuCoord.y * mcuSize );

  for ( int v = 0 ; v < mcuSize ; v++ ) {
    for ( int u = 0 ; u < mcuSize ; u++ ) {
      double a = 0; // 求める係数
      // f_uy の u 列目のベクトルと v 列目の係数ベクトルの内積を求める
      for ( int y = 0 ; y < mcuSize ; y++ )
        a += f_uy( u, y ) * coeff( v, y );
      if ( v == 0 ) a /= sqrt( 2 );
      (*mcu)( sp.x + u, sp.y + v ) = a * 2 / mcuSize;
    }
  }
}

/*
  MCU_Encode : 離散コサイン変換(DCT)

  yuv は YUVデータの一成分からなる二次元配列を表し、DCT 計算をして結果を mcu に返す。
  mcuCoord は対象となる MCU の位置を表す。
  間引き(サンプリング)処理も含めて行ない、skip は間引き量を表す。
*/
void MCU_Encode( const Matrix< double >& yuv, Matrix< double >* mcu, int mcuSize, Coord< int > skip )
{
  assert( &yuv != 0 );
  assert( mcu != 0 );

  // 画像 ( YUV データ ) のサイズ
  Coord< int > gSize = yuv.size();
  // X 成分の和を先に計算した結果
  SquareMatrix< double > f_uy( mcuSize );
  // 係数行列
  SquareMatrix< double > coeff( mcuSize );
  CalcCoeff( &coeff );

  Coord< int > mcuCount = GetMCUCount( gSize, mcuSize, skip ); // MCU の X,Y 方向の数を求める

  Coord< int > mcuCoord;
  mcu->assign( Coord< int >( mcuCount.x * mcuSize, mcuCount.y * mcuSize ), 0.0 );
  for ( mcuCoord.y = 0 ; mcuCoord.y < mcuCount.y ; ++( mcuCoord.y ) ) {
    for ( mcuCoord.x = 0 ; mcuCoord.x < mcuCount.x ; ++( mcuCoord.x ) ) {
      dct_x( yuv, coeff, &f_uy, mcuCoord, skip );
      dct_y( f_uy, coeff, mcuCoord, mcu );
    }
  }
}

/*
  idct_y : Y 成分に対する離散コサイン逆変換(IDCT)

  mcu は対象の MCU を指す反復子を表し、係数行列 coeff を元に IDCT 計算をして
  結果を f_uy に返す。
*/
void idct_y( const Matrix< double >& mcu, Coord< int > mcuCoord, const SquareMatrix< double >& coeff, SquareMatrix< double >* f_uy )
{
  // MCU のサイズ
  int mcuSize = f_uy->size();

  // mcu からの読み込み開始位置
  Coord< int > sp( mcuCoord.x * mcuSize, mcuCoord.y * mcuSize );

  for ( int y = 0 ; y < mcuSize ; y++ ) {
    for ( int u = 0 ; u < mcuSize ; u++ ) {
      double a = 0; // 求める係数
      // u 列目の MCU ベクトルと y 行目の係数ベクトルの内積を求める
      for ( int v = 0 ; v < mcuSize ; v++ ) {
        double d = mcu( sp.x + u, sp.y + v ) * coeff( v, y );
        if ( v == 0 ) d /= sqrt( 2 );
        a += d;
      }
      (*f_uy)( u, y ) = a;
    }
  }
}

/*
  fill_yuv : YUV データの書き込み

  c を始点として data を yuv 上に書き込む。
  間引きがある場合は、間引き数 skip 分だけ同じデータを書き込む。
*/
void fill_yuv( double data, Matrix< double >* yuv, Coord< int > c, Coord< int > skip )
{
  // 画像のサイズ
  Coord< int > gSize( yuv->size() );

  for ( int j = 0 ; j < skip.y ; j++ ) {
    if ( c.y + j >= gSize.y ) break;
    for ( int i = 0 ; i < skip.x ; i++ ) {
      if ( c.x + i >= gSize.x ) break;
      (*yuv)( c.x + i, c.y + j ) = data;
    }
  }
}

/*
  idct_x : X 成分に対する離散コサイン逆変換(IDCT)

  f_uy は X 方向の成分で計算した結果を表し、係数行列 coeff を元に IDCT 計算をして結果を yuv に書き込む。
  mcuCoord は対象の MCU の位置を、skip は間引き数をそれぞれ表す。
*/
void idct_x( const SquareMatrix< double >& f_uy, const SquareMatrix< double >& coeff, Matrix< double >* yuv, Coord< int > mcuCoord, Coord< int > skip )
{
  // MCU のサイズ
  int mcuSize = f_uy.size();
  // 画像のサイズ
  Coord< int > gSize( yuv->size() );
  // 画像(YUV成分)に書き込む位置
  Coord< int > gc( 0, mcuCoord.y * mcuSize * skip.y );

  for ( int y = 0 ; y < mcuSize ; y++ ) {
    if ( gc.y >= gSize.y ) break;
    gc.x = mcuCoord.x * mcuSize * skip.x;
    for ( int x = 0 ; x < mcuSize ; x++ ) {
      if ( gc.x >= gSize.x ) break;
      double a = 0; // 求める係数
      // f_uy の y 行目のベクトルと x 行目の係数ベクトルの内積を求める
      for ( int u = 0 ; u < mcuSize ; u++ ) {
        double d = f_uy( u, y ) * coeff( u, x );
        if ( u == 0 ) d /= sqrt( 2 );
        a += d;
      }
      fill_yuv( a * 2 / mcuSize, yuv, gc, skip );
      gc.x += skip.x;
    }
    gc.y += skip.y;
  }
}

/*
  MCU_Decode : 離散コサイン逆変換(IDCT)

  mcu のデータを IDCT 変換し、結果を yuv に書き込む。
  mcuSize は MCU のサイズ、skip は間引き量をそれぞれ表す。
*/
void MCU_Decode( const Matrix< double >& mcu, Matrix< double >* yuv, int mcuSize, Coord< int > skip )
{
  assert( &mcu != 0 );
  assert( yuv != 0 );

  // 画像 ( YUV データ ) のサイズ
  Coord< int > gSize = yuv->size();
  // X 成分の和を先に計算した結果
  SquareMatrix< double > f_uy( mcuSize );
  // 係数行列
  SquareMatrix< double > coeff( mcuSize );
  CalcCoeff( &coeff );

  Coord< int > mcuCount = GetMCUCount( gSize, mcuSize, skip ); // MCU の X,Y 方向の数を求める

  Coord< int > mcuCoord;
  for ( mcuCoord.y = 0 ; mcuCoord.y < mcuCount.y ; ++( mcuCoord.y ) ) {
    for ( mcuCoord.x = 0 ; mcuCoord.x < mcuCount.x ; ++( mcuCoord.x ) ) {
      idct_y( mcu, mcuCoord, coeff, &f_uy );
      idct_x( f_uy, coeff, yuv, mcuCoord, skip );
    }
  }
}

/*
  RGB成分用パレット構造体
*/
struct RGB
{
  // 型宣言
  typedef unsigned char primary_type; // RGB各成分の型

  // 定数宣言
  static const primary_type MAX = UCHAR_MAX; // RGB各成分の最大値

  // 公開メンバ変数
  primary_type r; // 赤成分
  primary_type g; // 緑成分
  primary_type b; // 青成分

  /* 静的メンバ関数 */

  // Check_RGBPrimary : 値 p が RGB::primary_type の値の範囲内にあるかを判定し、そうでなければ範囲内に収めて返す
  template< class T >
  static RGB::primary_type Check_RGBPrimary( T p )
  {
    if ( p < 0 ) p = 0;
    if ( p > RGB::MAX ) p = RGB::MAX;

    return( p );
  }

  /* コンストラクタ */

  // RGB コンストラクタ : RGB各成分からの構築
  RGB( primary_type red = primary_type(), primary_type green = primary_type(), primary_type blue = primary_type() )
    : r( red ), g( green ), b( blue ) {}

  /* RGB → YUV 成分変換 */

  // toY : RGB各成分から Y 成分を求める
  //
  // Y 成分を求める計算式は Y = 0.299R + 0.587G + 0.114B
  double toY() const
  { return( 0.299 * r + 0.587 * g + 0.114 * b ); }

  // toU : RGB各成分から U 成分を求める
  //
  // U 成分を求める計算式は U = -0.1687R - 0.3313G + 0.5B
  double toU() const
  { return( -0.1687 * r - 0.3313 * g + 0.5 * b ); }

  // toV : RGB各成分から V 成分を求める
  //
  // V 成分を求める計算式は V = 0.5R - 0.4187G - 0.0813B
  double toV() const
  { return( 0.5 * r - 0.4187 * g - 0.0813 * b ); }
};

// toR : YUV(YCbCr) 各成分から R 成分を求める
//
// R 成分を求める計算式は R = Y + 1.402V
RGB::primary_type toR( double y, double u, double v )
{
  return( RGB::Check_RGBPrimary( y + 1.402 * v + 0.5 ) );
}

// toG : YUV(YCbCr) 各成分から G 成分を求める
//
// G 成分を求める計算式は G = Y - 0.34414U - 0.71414V
RGB::primary_type toG( double y, double u, double v )
{
  return( RGB::Check_RGBPrimary( y - 0.34414 * u - 0.71414 * v + 0.5 ) );
}

// toB : YUV(YCbCr) 各成分から B 成分を求める
//
// B 成分を求める計算式は G = Y + 1.772U
RGB::primary_type toB( double y, double u, double v )
{
  return( RGB::Check_RGBPrimary( y + 1.772 * u + 0.5 ) );
}