/*
 * STUDUINO
 * (C) 2014 Artec Corporation
 * 
 * 本プログラムをArduino IDEで実行する場合、Studuinoライブラリを
 * インストールしなければなりません。
 * 
 * Studuinoライブラリのインストール方法は、下記のホームページを
 * 参考にしてください。
 * http://www.artec-kk.co.jp/studuino
 * 
 * 下記に主要な関数を説明します
 * --------------------------------------
 * ■ artecRobotSetup関数：接続パーツ等の初期化関数です
 * ■ artecRobotMain関数 ：「制御スタート」ブロックに接続したプログラムです
 * ■ ARSR_*関数         ：「関数ブロック」で作成したプログラムです
 * --------------------------------------
 */
#include <Arduino.h>
#include <Servo.h>
#include <Wire.h>
#include <MMA8653.h>
#include "Studuino.h"
// **********************************************************************
// 宣言
// **********************************************************************
// リストの要素
struct cell_t{
  struct cell_t* next;
  float data;
};
typedef cell_t cell;
// **********************************************************************
// 定義
// **********************************************************************
#define DCMPWR(power)	((byte)(min(max(0, ((float)(power) * 2.55)),255)))

#define BMIN    (0)     // Blockプログラミング環境側のセンサーの最小値
#define BMAX    (100)   // Blockプログラミング環境側のセンサーの最大値
#define ANAMIN  (0)     // Studuino基板のアナログセンサーの最小値
#define ANAMAX  (1023)  // Studuino基板のアナログセンサーの最大値
#define ACCMIN  (-128)  // Studuino基板の加速度センサーの最小値
#define ACCMAX  (127)   // Studuino基板の加速度センサーの最大値
#define PUSHSWITCH(port)       (board.GetPushSwitchValue(port))
#define TOUCH_SENSOR(port)     (board.GetTouchSensorValue(port))
#define LIGHT_SENSOR(port)     (map(board.GetLightSensorValue(port), ANAMIN, ANAMAX, BMIN, BMAX))
#define SOUND_SENSOR(port)     (map(board.GetSoundSensorValue(port), ANAMIN, ANAMAX, BMIN, BMAX))
#define IRPHOTOREFLECTOR(port) (map(board.GetIRPhotoreflectorValue(port), ANAMIN, ANAMAX, BMIN, BMAX))
#define ACCELEROMETER(axis)    (map(board.GetAccelerometerValue(axis), ACCMIN, ACCMAX, BMIN, BMAX))

const byte SQRT  =  (0);   // √n
const byte ABS   =  (1);   // |n|
const byte SIN   =  (2);   // sin(n)
const byte COS   =  (3);   // cos(n)
const byte TAN   =  (4);   // tan(n)
const byte LN    =  (5);   // loge
const byte LOG   =  (6);   // log10
const byte POWE  =  (7);   // e^
const byte POW10 =  (8);   // 10^

const word BTONE[] = {
  BZR_C3,  BZR_CS3, BZR_D3,  BZR_DS3, BZR_E3,  BZR_F3,  BZR_FS3, BZR_G3,  BZR_GS3, BZR_A3,  BZR_AS3, BZR_B3,  
  BZR_C4,  BZR_CS4, BZR_D4,  BZR_DS4, BZR_E4,  BZR_F4,  BZR_FS4, BZR_G4,  BZR_GS4, BZR_A4,  BZR_AS4, BZR_B4,  
  BZR_C5,  BZR_CS5, BZR_D5,  BZR_DS5, BZR_E5,  BZR_F5,  BZR_FS5, BZR_G5,  BZR_GS5, BZR_A5,  BZR_AS5, BZR_B5,  
  BZR_C6,  BZR_CS6, BZR_D6,  BZR_DS6, BZR_E6,  BZR_F6,  BZR_FS6, BZR_G6,  BZR_GS6, BZR_A6,  BZR_AS6, BZR_B6,  
  BZR_C7,  BZR_CS7, BZR_D7,  BZR_DS7, BZR_E7,  BZR_F7,  BZR_FS7, BZR_G7,  BZR_GS7, BZR_A7,  BZR_AS7, BZR_B7,  
  BZR_C8,  
};
#define TONENUM	((sizeof(BTONE)/sizeof(word))-1)
#define BHZ(num)  (BTONE[(byte)(min(max(0, (num-48)),TONENUM))])

// for Servomotor calibration
const byte SVCD2  = (4);
const byte SVCD4  = (5);
const byte SVCD7  = (6);
const byte SVCD8  = (7);
const byte SVCD9  = (0);
const byte SVCD10 = (1);
const byte SVCD11 = (2);
const byte SVCD12 = (3);
// Min/Max of servomotor's degree
#define SVRANGE(deg)	(min(max(0, (deg)), 180))
#define SYNCSVRANGE(dly)	(min(max(0, (dly)), 20))

// **********************************************************************
// プロトタイプ宣言
// **********************************************************************
// リスト
int listDelete(struct cell_t* p, int pos);              // リストの要素を削除
int listAdd(struct cell_t* p, float data);              // リストに要素を追加
int listInsert(struct cell_t *p, int pos, float data);  // リストに要素を挿入
int listReplace(struct cell_t *p, int pos, float data); // リストの要素を置き換える
int listLength(struct cell_t *p);                       // リストの長さを取得
float listItem(struct cell_t *p, int pos);              // リストの要素を取得
bool listIsContain(struct cell_t *p, float data);       // リストにデータが含まれるかどうかの確認
// 丸め処理
int scratchRound(float arg);
// 数学処理
float math(byte opeID, float arg);              // 算術処理
// タイマー処理関数
void resetTimer();
float getTimer();

// ロボットセットアップ処理
void artecRobotSetup();
// ロボットメイン処理
void artecRobotMain();

// **********************************************************************
// グローバル変数
// **********************************************************************
Studuino board;	// Studuino基板オブジェクト
unsigned long StartTime;	// For timer

// **********************************************************************
// プログラム
// **********************************************************************
