#include <Wire.h>
#include <EEPROM.h>

#define LED_R 5
#define LED_G 6
#define LED_B 7
#define SENSOR_1 A0
#define SENSOR_2 A1
#define SENSOR_3 A2
#define SENSOR_4 A3
#define SENSOR_5 A6
#define NUM_SENSOR 5
#define SW 8
#define RED 0
#define GREEN 1
#define BLUE 2
#define DARK 3
#define NUM_SAMPLE 4

static word sens_data[NUM_SENSOR][4];          // センサの読み取り値[センサ番号][色：0=R, 1=G, 2=B, 3=消灯]
static word correction_factor[NUM_SENSOR][3];  // 白補正の係数
static word corrected_data[NUM_SENSOR][3];    // 補正後の値
static word corrected_data_white[NUM_SENSOR];        // 白
static int temp, temp2;
static long hue[NUM_SENSOR], hue255_temp;
static word hue255[3];
static word rgb255[NUM_SENSOR][3];
static word brt[NUM_SENSOR];


word s_buf_w[12];        // 送信データのバッファ（wordサイズ）
byte r_buf[10];          // 受信データのバッファ
byte s_buf[24];          // 送信データのバッファ
byte numBytes_command;    // コマンドのバイト数（2バイト目以降のバイト数）
byte command;            // マスタから受け取ったコマンド
int numReplyBytes;        // マスタに返すバイト数

void setup() {
  int i, j, k;
  byte temp_hb, temp_lb;

  Wire.begin(0x18);                     // I2Cスレーブアドレスを設定
  Wire.onReceive(i2cReceiveEvent);      // 割り込み関数の登録
  Wire.onRequest(i2cSendEvent);         // 割り込み関数の登録

  Serial.begin(38400);

  pinMode(LED_R, OUTPUT);
  pinMode(LED_G, OUTPUT);
  pinMode(LED_B, OUTPUT);
  pinMode(SENSOR_1, INPUT);
  pinMode(SENSOR_2, INPUT);
  pinMode(SENSOR_3, INPUT);
  pinMode(SENSOR_4, INPUT);
  pinMode(SENSOR_5, INPUT);
  pinMode(SW, INPUT_PULLUP);

  // 補正値をEEPROMから読み出す
  k = 0;
  for (i = 0; i < NUM_SENSOR; i++) {
    for (j = 0; j < 3; j++) {
      temp_lb = EEPROM.read(k);
      k++;
      temp_hb = EEPROM.read(k);
      k++;
      correction_factor[i][j] = (word)temp_hb << 8;
      correction_factor[i][j] += (word)temp_lb;
    }
  }
  for (i = 0; i < NUM_SENSOR; i++) {
    temp_lb = EEPROM.read(k);
    k++;
    temp_hb = EEPROM.read(k);
    k++;
    corrected_data_white[i] = (word)temp_hb << 8;
    corrected_data_white[i] += (word)temp_lb;
  }
}

void loop() {
  int i, j, k;
  int index_of_max, index_of_min;

  byte temp_hb, temp_lb;

  digitalWrite(LED_R, LOW);
  digitalWrite(LED_G, LOW);
  digitalWrite(LED_B, LOW);
  delayMicroseconds(600);
  sens_data[0][DARK] = analogRead(A0);
  sens_data[1][DARK] = analogRead(A1);
  sens_data[2][DARK] = analogRead(A2);
  sens_data[3][DARK] = analogRead(A3);
  sens_data[4][DARK] = analogRead(A6);
  digitalWrite(LED_R, HIGH);
  digitalWrite(LED_G, LOW);
  digitalWrite(LED_B, LOW);
  delayMicroseconds(600);
  sens_data[0][RED] = analogRead(A0);
  sens_data[1][RED] = analogRead(A1);
  sens_data[2][RED] = analogRead(A2);
  sens_data[3][RED] = analogRead(A3);
  sens_data[4][RED] = analogRead(A6);
  digitalWrite(LED_R, LOW);
  digitalWrite(LED_G, HIGH);
  digitalWrite(LED_B, LOW);
  delayMicroseconds(600);
  sens_data[0][GREEN] = analogRead(A0);
  sens_data[1][GREEN] = analogRead(A1);
  sens_data[2][GREEN] = analogRead(A2);
  sens_data[3][GREEN] = analogRead(A3);
  sens_data[4][GREEN] = analogRead(A6);
  digitalWrite(LED_R, LOW);
  digitalWrite(LED_G, LOW);
  digitalWrite(LED_B, HIGH);
  delayMicroseconds(600);
  sens_data[0][BLUE] = analogRead(A0);
  sens_data[1][BLUE] = analogRead(A1);
  sens_data[2][BLUE] = analogRead(A2);
  sens_data[3][BLUE] = analogRead(A3);
  sens_data[4][BLUE] = analogRead(A6);
  digitalWrite(LED_R, LOW);
  digitalWrite(LED_G, LOW);
  digitalWrite(LED_B, LOW);

  for (i = 0; i < NUM_SENSOR; i++) {                    // 消灯時の電圧を引く
    for (j = 0; j < 3; j++) {
      if (sens_data[i][j] > sens_data[i][DARK]) {
        sens_data[i][j] -= sens_data[i][DARK];
      } else {
        sens_data[i][j] = 0;                            // マイナスにならないようにする
      }
    }
  }

  for (i = 0; i < NUM_SENSOR; i++) {
    for (j = 0; j < 3; j++) {
      corrected_data[i][j] = sens_data[i][j] * correction_factor[i][j];    // 白補正 16倍のまま
    }
  }

  for (i = 0; i < NUM_SENSOR; i++) {                            // RGBを0～255に正規化
    temp = find_index_of_max(&(corrected_data[i][0]), 3);         // 最大値（の添え字）を探す
    temp2 = corrected_data_white[i] / 256;
    brt[i] = 0;
//    Serial.print(i, DEC);
//    Serial.print(":");
    for (j = 0; j < 3; j++) {
      rgb255[i][j] = map(corrected_data[i][j], 0, corrected_data[i][temp], 0, 255);
//      Serial.print(rgb255[i][j], DEC);
//      Serial.print(" ");
      brt[i] += corrected_data[i][j] / temp2;
    }
    brt[i] = brt[i] * 2 / 3;
    s_buf_w[i + 5] = brt[i];
  }
//  Serial.println("");

  for (i = 0; i < NUM_SENSOR; i++) {
    index_of_max = find_index_of_max(&(rgb255[i][0]), 3);
    index_of_min = find_index_of_min(&(rgb255[i][0]), 3);
    switch (index_of_max) {
      case 0:
        hue[i] = (long)((60 * ((long)rgb255[i][1] - (long)rgb255[i][2]) / ((long)rgb255[i][index_of_max] - (long)rgb255[i][index_of_min]) + 360) % 360);
        break;
      case 1:
        hue[i] = (long)((60 * ((long)rgb255[i][2] - (long)rgb255[i][0]) / ((long)rgb255[i][index_of_max] - (long)rgb255[i][index_of_min])) + 120);
        break;
      case 2:
        hue[i] = (long)((60 * ((long)rgb255[i][0] - (long)rgb255[i][1]) / ((long)rgb255[i][index_of_max] - (long)rgb255[i][index_of_min])) + 240);
        break;
      default:
        hue[i] = 0;
        break;
    }
//    Serial.print(hue[i], DEC);
//    Serial.print(" ");
    if ((rgb255[i][index_of_max] * 10) < (rgb255[i][index_of_min] * 17)) {
      hue[i] = 1023;
    }
    s_buf_w[i] = hue[i];
  }
//  Serial.println("");
  for (i = 0; i < 10; i++) {
    Serial.print(s_buf_w[i], DEC);
    Serial.print(",");
  }
  Serial.println("");
//  for (i = 0; i < 5; i++) {
//    for (j = 0; j < 4; j++) {
//      Serial.print(sens_data[i][j], DEC);
//      Serial.print(",");
//    }
//    Serial.print(":");
//  }
//  Serial.println("");

  // 白補正
  if (digitalRead(SW) == LOW) {
    k = 0;
    for (i = 0; i < NUM_SENSOR; i++) {
      temp = find_index_of_max(&(sens_data[i][0]), 3);
      for (j = 0; j < 3; j++) {
        correction_factor[i][j] = (sens_data[i][temp] * 16) / sens_data[i][j];
        EEPROM.write(k, lowByte(correction_factor[i][j]));
        k++;
        EEPROM.write(k, highByte(correction_factor[i][j]));
        k++;
      }
    }
    for (i = 0; i < NUM_SENSOR; i++) {
      EEPROM.write(k, lowByte(corrected_data[i][0]));
      k++;
      EEPROM.write(k, highByte(corrected_data[i][0]));
      k++;
    }
    digitalWrite(LED_R, LOW);
    digitalWrite(LED_G, LOW);
    digitalWrite(LED_B, LOW);
    delay(200);
    digitalWrite(LED_R, HIGH);
    digitalWrite(LED_G, LOW);
    digitalWrite(LED_B, LOW);
    delay(200);
    digitalWrite(LED_R, LOW);
    digitalWrite(LED_G, HIGH);
    digitalWrite(LED_B, LOW);
    delay(200);
    digitalWrite(LED_R, LOW);
    digitalWrite(LED_G, LOW);
    digitalWrite(LED_B, HIGH);
    delay(200);
  }

}


// ---- 配列の中の最大値の添え字を探す ----
int find_index_of_max(word *data_p, int data_num)
{
  int i;
  int suffix_of_max;

  suffix_of_max = 0;

  for (i = 1; i < data_num; i++) {
    if (*(data_p + suffix_of_max) < * (data_p + i)) {
      suffix_of_max = i;
    }
  }

  return suffix_of_max;
}

// ---- 配列の中の最小値の添え字を探す ----
int find_index_of_min(word *data_p, int data_num)
{
  int i;
  int suffix_of_min;

  suffix_of_min = 0;

  for (i = 1; i < data_num; i++) {
    if (*(data_p + suffix_of_min) > *(data_p + i)) {
      suffix_of_min = i;
    }
  }

  return suffix_of_min;
}

// ---- 配列の中の最大値の添え字を探す（signed long用） ----
int find_index_of_max_sl(long *data_p, int data_num)
{
  int i;
  int suffix_of_max;

  suffix_of_max = 0;

  for (i = 1; i < data_num; i++) {
    if (*(data_p + suffix_of_max) < * (data_p + i)) {
      suffix_of_max = i;
    }
  }

  return suffix_of_max;
}

// ---- 配列の中の最小値の添え字を探す（signed long用） ----
int find_index_of_min_sl(long *data_p, int data_num)
{
  int i;
  int suffix_of_min = 0;

  for (i = 1; i < data_num; i++) {
    if (*(data_p + suffix_of_min) > *(data_p + i)) {
      suffix_of_min = i;
    }
  }

  return suffix_of_min;
}

// ==== データ受信時の処理 ====
void i2cReceiveEvent(int numBytes) {
  int n = 0;
  int i;

  while (0 < Wire.available()) {
    r_buf[n] = (byte)Wire.read();
    n++;
  }
  command = r_buf[1];                // 受信データの2バイト目がコマンド
  switch (command) {
    case 0x80:
      numReplyBytes = 10;
      for (i = 0; i < 8; i++) {
        s_buf[i] = 0x01;
      }
      for (i = 8; i < 10; i++) {
        s_buf[i] = 0x01;
      }
      break;
    case 0x13:
      break;
    case 0x14:
      break;
    case 0x81:
      if (r_buf[2] == 0x0a) {
        numReplyBytes = 22;
        for (i = 0; i < 10; i++) {
          s_buf[i * 2] = lowByte(s_buf_w[i]);
          s_buf[i * 2 + 1] = highByte(s_buf_w[i]);
        }
        s_buf[20] = 0;
        s_buf[21] = 0;
      }
      else {
        numReplyBytes = 2;
        s_buf[0] = lowByte(s_buf_w[r_buf[2]]);
        s_buf[1] = highByte(s_buf_w[r_buf[2]]);
      }
      break;
    default:
      break;
  }
}

// ==== データ送信時の処理 ====
void i2cSendEvent() {
  Wire.write(s_buf, numReplyBytes);
}