// I2C通信ライブラリを取り込む
#include <Wire.h>
#include <WireExt.h>
// カメラモジュールのアドレス設定
const int configAddress = 0x3D;
const int cameraAddress = 0x3E;
int loop_count;

void setup() {  
  Serial.begin(115200);  //シリアル通信開始  
  Serial.print("Begin\n");
  
  // I2C通信開始
  Wire.begin();
  
  byte cam_config[][2] = {
      {0x02, 0x00},  // Set Camera Active
      {0x02, 0x40},  // Set Camera Reset
      {0x03, 0x00},  // PLL
      {0x02, 0x00},  // Set Camera Active
      {0x0B, 0x00},  // White Line OFF 
      {0x58, 0x20},  // Exposure Time 
      {0x05, 0x80},  // Frame Rate Quarter(00)/Half(40)/Full(80)
      {0x1A, 0xFF},  // HCOUNT = 0x3FF = 1023 
      {0x1B, 0xB3},  // VCOUNT = 0x21B = 539
      {0x1C, 0xA1},  // 
      {0x11, 0x4A},  // Changed b/c of PICSIZE 
      {0x14, 0x23},  // Changed b/c of PICSIZE 
      {0x04, 0x0B},  // RGB  160x120  OUT ON 
      {0x1F, 0x0C},  // SPCOUNT = 0xBC3 = 3011 // Doesn't match formula? 
      {0x1E, 0x23},  // SPCOUNT[7:0] 
      {0x0E, 0x00},  // According to the AppNote it should be 0xAC for this PICSIZE?
      };
  
  int i;
  for(i = 0; i < sizeof(cam_config) /  sizeof(cam_config[0]); i++){
    Wire.beginTransmission(configAddress);
    Wire.send(cam_config[i][0]); Wire.send(cam_config[i][1]);
    Wire.endTransmission();
  }
  
  loop_count = 0;
}

void loop() {  
  byte buf = 0;
  
  if(loop_count < 4){
    // 4枚は捨てる
    if(WireExt.beginReception(cameraAddress) >= 0){
      WireExt.endReception();
    }
    loop_count++;
    return;
  }
  
  Serial.print("Hello");
  
  if(WireExt.beginReception(cameraAddress) >= 0){
    unsigned long count;
    {
      byte header_count = 0;
      while(header_count < 4){
        buf = WireExt.get_byte();
        *((byte *)(&count) + header_count) = buf;  // Little Endian
        Serial.print(buf);
        header_count++;
      }
    }
    
    while(count-- > 0){
      Serial.print(WireExt.get_byte());
    }
    WireExt.endReception();
  }
  
  // 処理のために少し待つ
  delay(100);
}