/*

SensorControlModuleDIO.ino

copyright(c) 2013 All Blue System

----------------------------------------------------------------------
■始めに
----------------------------------------------------------------------

このスケッチは、シリアルポートからFirmata プロトコルでリクエストされた
コマンドによってデジタル I/O ポートの操作を行います。また、I2C マスター
としてI2C スレーブデバイスをコントロールすることもできます。

標準で定義されたプロトコルの他に、Firmata の SYSEX 拡張フォーマットを
使用して独自にコマンドを定義しています。
詳細は、"サポートしている Firmata プロトコル" の項目をご覧下さい。

----------------------------------------------------------------------
■使用するにあたって
----------------------------------------------------------------------

個人利用または商用利用を問わずにこのファイルを自由に使用できます。
ファイルの内容を改変したり、一部分を取り出して利用することもできます。

オールブルーシステムは、このファイルを使用したことによる損害等につい
て一切保障できません。

----------------------------------------------------------------------
■このファイルの動作環境
----------------------------------------------------------------------

このファイルは Arduino Duemilanove 328 互換機で動作確認を行っています。
ATmega328P以外の CPU を使用している Arduino や FT232RL以外の
USB-シリアル変換デバイスを使用した Arduino 互換ボードでの動作確認は
していません。

このファイルは Arduino IDE ver1.0.1-1.0.4 for Windows で動作確認を行っ
ています。これより古いバージョンの Arduino IDE では動作しない場合があり
ます。

また Windows 以外の OS では動作確認していません。

----------------------------------------------------------------------
■このファイルを使用する前に ArduinoIDE のファイルを修正してください
----------------------------------------------------------------------

Arduino IDE ver1.0.1-1.0.4 for Windows では下記のパッチを当てる必要があ
ります(ver1.0.1 の場合を例にして説明します)

(1) Firmata 標準ライブラリの最大受信データ数を 32バイトから 約70 バイト
まで増やす。

arduino-1.0.1-windows/arduino-1.0.1/libraries/Firmata/Firmata.h ファイ
ル中の定義部分を下記の様に変更してください。日本語を使用する場合には
エディタは UTF-8(BOMなし)エンコード形式にしてください。

//#define MAX_DATA_BYTES 32 // max number of data bytes in non-Sysex...
#define MAX_DATA_BYTES 70   // Firmata で受信可能なパケットサイズを拡張

----------------------------------------------------------------------
■Arduino ピンコンフィギュレーション
----------------------------------------------------------------------
割り当てた機能 = 使用する Arduino ピン番号リスト

H/W SERIAL = 0,1
DIGITAL PORT(INPU/OUTPUT) = 2,3,4,5,6,7,8,9,10,11,12,13

	INPUT/OUTPUT のピンの割り当て変更は、SensorControlModuleDIO.ino 
	ファイル中の setup() 関数内のコメントを参照してください

I2C SDA = 18
I2C SCL = 19

----------------------------------------------------------------------
■サポートしている Firmata プロトコル
----------------------------------------------------------------------
 
[DIGITAL_MESSAGE]
   
   標準プロトコルで定義されているプロトコル。
   PC から Arduinoボード側へのI/O ポート値設定のみに使用しています。
   
   -- DIGITAL_MESSAGE --

   0  DIGITAL_MESSAGE      (0x90) + port#
   1  port value bits 0-6  (LSB)
   2  port value bits 7-13 (MSB)
  
[DIGITAL_PORTS_QUERY]
 
   全てのデジタルポートの現在の値を取得するためのリクエストコマンド
   
   -- DIGITAL_PORTS_QUERY --

   0  START_SYSEX          (0xF0)
   1  DIGITAL_PORTS_QUERY  (0x61)
   2  END_SYSEX            (0xF7)
  
[DIGITAL_PORTS_REPLY]

   DIGITAL_PORTS_QUERY コマンドのリプライメッセージパケット。

   -- DIGITAL_PORTS_REPLY --

   0  START_SYSEX          (0xF0)
   1  DIGITAL_PORTS_REPLY  (0x62)
   3  port#0 bits 0-6      (LSB)
   4  port#0 bits 7-13     (MSB)
   ..
   5  port#n bits 0-6      (LSB)
   6  port#n bits 7-13     (MSB)
   7  END_SYSEX            (0xF7)

[DIGITAL_WRITE_PIN]

  デジタルポートの指定したピンの値をセットする
  
   -- DIGITAL_WRITE_PIN --

   0  START_SYSEX          (0xF0)
   1  DIGITAL_WRITE_PIN    (0x63)
   3  pin number           (0x02-0x0D)
   4  pin value            (0/1)
   5  END_SYSEX            (0xF7)

[DIGITAL_CHANGE_DETECT]

   I/O 値が更新された場合に、PC 側に最新のI/O ポート値を送信する

   -- DIGITAL_CHANGE_DETECT --

   0  START_SYSEX          (0xF0)
   1  DIGITAL_CHANGE_DETECT(0x6B)
   2  port#0 diff bits 0-6 (LSB)
   3  port#0 diff bits 7-13(MSB)
   4  port#0 bits 0-6      (LSB)
   5  port#0 bits 7-13     (MSB)
   ..
   6  port#n diff bits 0-6 (LSB)
   7  port#n diff bits 7-13(MSB)
   8  port#n bits 0-6      (LSB)
   9  port#n bits 7-13     (MSB)
  10  END_SYSEX            (0xF7)


[DIGITAL_CHANGE_MASK]

   DIGITAL_CHANGE_DETECTで検出対象とするポートのビットマスク値を指定する
   ビットが 1 の場合に DIGITAL_CHANGE_DETECT 検出対象となる
   初期値は、全ポートのビットマスクがセットされて検出対象になっています。
   SensorControlModuleDIO.ino ファイル中の setup() 関数内のコメントを
   参照してください

   -- DIGITAL_CHANGE_MASK --

   0  START_SYSEX          (0xF0)
   1  DIGITAL_CHANGE_MASK  (0x6C)
   1  port number	  	   (0x00-0x7F)
   2  port mask bits 0-6   (LSB)
   3  port mask bits 7-13  (MSB)
  10  END_SYSEX            (0xF7)


[I2C_WRITE_AND_READ_QUERY]
  
   I2C スレーブデバイスにデータを書き込んだ後、同一スレーブデバイスから
   データを読み込む
  
   -- I2C_WRITE_AND_READ_QUERY --

   0  START_SYSEX              (0xF0)
   1  I2C_WRITE_AND_READ_QUERY (0x65)
   2  7bit slave address       (0x01-0x7F)
   3  read data count          (0-32)
   4  write#1 bits 0-6         (LSB)
   5  write#1 bits 7-13        (MSB)
   ..
   6  write#n bits 0-6         (LSB)
   7  write#n bits 7-13        (MSB) 
   8  END_SYSEX                (0xF7)
  
   * 書き込みデータ write#n の最大エントリ数は 31

  
[I2C_READ_REPLY]

   I2C_READ_QUERY または I2C_WRITE_AND_READ_QUERYコマンドの
   リプライメッセージパケット。スレーブデバイスから読み込んだ
   データが格納される

   -- I2C_READ_REPLY --

   0  START_SYSEX          (0xF0)
   1  I2C_READ_REPLY       (0x66)
   3  read#1 bits 0-6      (LSB)
   4  read#1 bits 7-13     (MSB)
   ..
   5  read#n bits 0-6      (LSB)
   6  read#n bits 7-13     (MSB)
   7  END_SYSEX            (0xF7)
   
   * 読み込みデータ read#n の最大エントリ数は 32

[I2C_WRITE_COMMAND]
  
   I2C スレーブデバイスにデータを書き込む
  
   -- I2C_WRITE_COMMAND --

   0  START_SYSEX              (0xF0)
   1  I2C_WRITE_COMMAND        (0x67)
   2  7bit slave address       (0x01-0x7F)
   4  write#1 bits 0-6         (LSB)
   5  write#1 bits 7-13        (MSB)
   ..
   6  write#n bits 0-6         (LSB)
   7  write#n bits 7-13        (MSB) 
   8  END_SYSEX                (0xF7)
  
   * 書き込みデータ write#n の最大エントリ数は 31

[I2C_READ_QUERY]
  
   I2C スレーブデバイスからデータを読み込む
  
   -- I2C_READ_QUERY --

   0  START_SYSEX              (0xF0)
   1  I2C_READ_QUERY           (0x68)
   2  7bit slave address       (0x01-0x7F)
   3  read data count          (0-32)
   8  END_SYSEX                (0xF7)


このリスト以外にも標準ライブラリで提供される VERSION リクエストコマンド
(0xF0,0x79,0xF7)とそのリプライメッセージが使用できます

*/

#include <Firmata.h>
#include <Wire.h>

//  デジタルポート数の定義
//  ATmega328 を使用した Arduino ボードの場合は 2 になる
#define TOTAL_DIGITAL_PORTS             ((TOTAL_PINS - TOTAL_ANALOG_PINS + 7) / 8)

// SYSEX コマンドの定義
#define DIGITAL_PORTS_QUERY      0x61
#define DIGITAL_PORTS_REPLY      0x62
#define DIGITAL_WRITE_PIN        0x63

#define I2C_WRITE_AND_READ_QUERY    0x65
#define I2C_READ_REPLY         0x66
#define I2C_WRITE_COMMAND      0x67
#define I2C_READ_QUERY         0x68

#define DIGITAL_CHANGE_DETECT  0x6B
#define DIGITAL_CHANGE_MASK    0x6C

// I2C データ書き込みリクエストと受信データリプライ用のデータバッファ
#define I2C_DATA_BUFF_SIZE 64
volatile byte i2c_req_size;
byte i2c_req_buff[I2C_DATA_BUFF_SIZE];
volatile byte i2c_rpl_size;
byte i2c_rpl_buff[I2C_DATA_BUFF_SIZE];

// デジタルポートの値が変更されかどうかを判断するための値保存用
volatile byte stable_prev_dio[TOTAL_DIGITAL_PORTS];
volatile byte stable_current_dio[TOTAL_DIGITAL_PORTS];
volatile byte prev_dio[TOTAL_DIGITAL_PORTS];
volatile byte current_dio[TOTAL_DIGITAL_PORTS];
volatile byte f_change_detect[TOTAL_DIGITAL_PORTS];
volatile uint8_t first_dio_sample;

// i2c_rpl_buff,i2c_rpl_size にセットされたバイト列を
// SYSEX コマンド I2C_READ_REPLY で送信する
void replayI2CReadData(void){
    Serial.write(START_SYSEX);
    Serial.write(I2C_READ_REPLY);
    for (byte i=0; i<i2c_rpl_size; i++) {
        Serial.write(i2c_rpl_buff[i] & 0x7F);        // LSB
        Serial.write((i2c_rpl_buff[i] >> 7) & 0x7F); // MSB
    }
    Serial.write(END_SYSEX);
}

// SYSEX コマンドの処理
void sysexCallback(byte command, byte argc, byte *argv)
{
    byte pval;
    byte i2c_slave_addr;
    byte read_cnt;
    
    switch(command) {
      case I2C_WRITE_COMMAND:
            i2c_slave_addr = argv[0];
            // prepare write data
            i2c_req_size = (argc - 1) / 2;             
            for(byte i=0;i < i2c_req_size;i++){
                i2c_req_buff[i] = argv[1 + i * 2] + (argv[1 + i * 2 + 1] << 7);
            }
            // スレーブデバイスに書き込み            
            Wire.beginTransmission(i2c_slave_addr);
            for(byte i=0;i < i2c_req_size;i++) Wire.write(i2c_req_buff[i]);
            Wire.endTransmission(true);
            break;

        case I2C_READ_QUERY:
            i2c_slave_addr = argv[0];
            read_cnt = argv[1];
            // スレーブデバイスからデータ取得
            if(read_cnt > 0){
                if(Wire.requestFrom((int)i2c_slave_addr,(int)read_cnt,true) == read_cnt){
                    for(byte i=0;i < read_cnt;i++) i2c_rpl_buff[i] = Wire.read();
                    i2c_rpl_size = read_cnt;
                } else {
                    i2c_rpl_size = 0; // read error
                }
            } else {
                // 書き込みのみ、ストップコンディションを送信
                Wire.endTransmission(true);
                i2c_rpl_size = 0;
            }
            // FIRMATA リプライデータ送信
            replayI2CReadData();
            break;

      case I2C_WRITE_AND_READ_QUERY:
            i2c_slave_addr = argv[0];
            read_cnt = argv[1];
            // 7ビット 2 バイトのエンコードデータをバイトに戻す
            i2c_req_size = (argc - 2) / 2;             
            for(byte i=0;i < i2c_req_size;i++){
                i2c_req_buff[i] = argv[2 + i * 2] + (argv[2 + i * 2 + 1] << 7);
            }
            // スレーブデバイスにデータを書き込む            
            Wire.beginTransmission(i2c_slave_addr);
            for(byte i=0;i < i2c_req_size;i++) Wire.write(i2c_req_buff[i]);
            // スレーブからデータを取得する
            if(read_cnt > 0){
                // REPEATED START コンディションが送信される
                Wire.endTransmission(false);
                // データ読み込み
                if(Wire.requestFrom((int)i2c_slave_addr,(int)read_cnt,true) == read_cnt){
                    for(byte i=0;i < read_cnt;i++) i2c_rpl_buff[i] = Wire.read();
                    i2c_rpl_size = read_cnt;
                } else {
                    i2c_rpl_size = 0; // read error
                }
            } else {
                // 書き込みのみ、ストップコンディションを送信 
                Wire.endTransmission(true);
                i2c_rpl_size = 0;
            }
            // FIRMATA リプライデータ送信t
            replayI2CReadData();
            break;
            
        case DIGITAL_PORTS_QUERY:
            Serial.write(START_SYSEX);
            Serial.write(DIGITAL_PORTS_REPLY);
            for (byte i=0; i<TOTAL_DIGITAL_PORTS; i++) {
                pval = readPort(i, 0xff);
                Serial.write(pval & 0x7F);        // LSB
                Serial.write((pval >> 7) & 0x7F); // MSB
            }
            Serial.write(END_SYSEX);
            break;
            
        case DIGITAL_WRITE_PIN:
            digitalWrite(argv[0],argv[1]);
            break;

        case DIGITAL_CHANGE_MASK:
            if(argv[0] < TOTAL_DIGITAL_PORTS){
                f_change_detect[argv[0]] = argv[1] + (argv[2] << 7); 
            }
            break;
    }
}

// FIRMATA DIGITAL_MESSAGE メッセージ受信時に I/O ポートの値を更新する
void digitalWriteCallback(byte port, int value)
{
    byte currentPinValue;
    if (port < TOTAL_DIGITAL_PORTS) {
        for(byte i=0; i<8; i++) {
            currentPinValue = (byte) value & (1 << i);
            digitalWrite(i + (port*8), currentPinValue);
        }
    }
}

// 定期的にコールされて、DIGITAL ポートの変化を検出した場合に
// FIRMATA メッセージを送信する
void checkAndReportIOTask(void){
    int   port_stable = 1;
    int   change_detect = 0;
    byte  diff_bits[TOTAL_DIGITAL_PORTS];

    // 前回取得したデジタルポーﾄの値を保存
    for (byte p=0; p<TOTAL_DIGITAL_PORTS; p++) {
	prev_dio[p] = current_dio[p];
    }
    // 現在のデジタルポートの値を取得
    for (byte p=0; p<TOTAL_DIGITAL_PORTS; p++) {
        current_dio[p] = readPort(p, 0xff);
    }    
    // prev_dio == current_dio ?  チャタリング、グリッチ除去
    for (byte p=0; p<TOTAL_DIGITAL_PORTS; p++) {
      if(prev_dio[p] != current_dio[p]) port_stable = 0;
    }      
    if(port_stable) {
        // リセット後に最初のサンプリング値を比較する時には現在のポート値を初期値として使う
        if (first_dio_sample) {
            first_dio_sample = 0;
            for (byte p=0; p<TOTAL_DIGITAL_PORTS; p++) {
	        stable_prev_dio[p] = current_dio[p];
	        stable_current_dio[p] = current_dio[p];
            }
        }
        // チャタリング、グリッチ除去後のポート値を保存      
        for (byte p=0; p<TOTAL_DIGITAL_PORTS; p++) {
	    stable_prev_dio[p] = stable_current_dio[p];
	    stable_current_dio[p] = current_dio[p];	
        }
	// change_detect で指定されたビット位置の値が、前回取得したポート値と違っているかを調べる
        for (byte p=0; p<TOTAL_DIGITAL_PORTS; p++) {
	    diff_bits[p] = ((stable_prev_dio[p] & f_change_detect[p]) ^ (stable_current_dio[p] & f_change_detect[p])) & 0xFF;
            if(diff_bits[p]) change_detect = 1;
        }
        // DIGITAL_CHANGE_DETECT メッセージ送信
	if(change_detect) {
            Serial.write(START_SYSEX);
            Serial.write(DIGITAL_CHANGE_DETECT);
            for (byte p=0; p<TOTAL_DIGITAL_PORTS; p++) {
                Serial.write(diff_bits[p] & 0x7F);        // LSB
                Serial.write((diff_bits[p] >> 7) & 0x7F); // MSB
                Serial.write(stable_current_dio[p] & 0x7F);        // LSB
                Serial.write((stable_current_dio[p] >> 7) & 0x7F); // MSB
            }
            Serial.write(END_SYSEX);
	}
    }
}

void setup()
{
    // Firmata ライブラリ初期設定、イベントハンドラ、ボーレート設定
    Firmata.setFirmwareNameAndVersion("SensorControlModule",1,0); // VERSION コマンドの応答データ
    Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback);
    Firmata.attach(START_SYSEX, sysexCallback);
    Firmata.begin(57600);

    // デジタルポート初期化

    // (config 1) pin#2-#13 デジタルポートを全て OUTPUT モードに初期設定する
    for(byte i = 2;i <= 13;i++){
        pinMode(PIN_TO_DIGITAL(i), OUTPUT);  
    }

    /*
    // (config 2) pin#2-7 のデジタルポートを INPUT_PULLUP, pin#8-#13 を OUTPUT モードに初期設定する
    for(byte i = 2;i <= 7;i++){
        pinMode(PIN_TO_DIGITAL(i), INPUT_PULLUP);  
    }
    for(byte i = 8;i <= 13;i++){
        pinMode(PIN_TO_DIGITAL(i), OUTPUT);  
    }
    */
    
    // DIGITAL ポート変化チェック用の変数を初期化
    first_dio_sample = 1;
    for (byte p=0; p<TOTAL_DIGITAL_PORTS; p++) {
	stable_prev_dio[p] = 0;
	stable_current_dio[p] = 0;
	prev_dio[p] = 0;
	current_dio[p] = 0;
        f_change_detect[p] = 0xff; // 全てのDITAILポート中のビット変化を検出対象とする
    }
    
    // I2C 初期設定
    Wire.begin();

    // ここからユーザーセットアップルーチンを記述する

}


void loop()
{
    static unsigned long tim_io = 0;
    
    // DIGITAL I/O の変化イベントデータをPCに送信
    if (millis() - tim_io > 10UL) {
        tim_io = millis();
        checkAndReportIOTask();
    }
    
    // FIRMATA メッセージ処理
    while(Firmata.available()) {
        Firmata.processInput();
    }

    // ここからユーザー処理ループを記述する

}

