/*
 * STUDUINO library
 * (C) 2014 Artec Corporation
 * 
 * 用Arduino IDE运行本程序时，必须先安装Studuino库。
 * 
 * Studuino库的安装方法，请参考以下网页。
 * http://www.artec-kk.co.jp/studuino
 */
#define DEF_I2C_GYRO
#define DEF_I2C_ACCEL
#include <Arduino.h>
#include <Servo.h>
#include <Wire.h>
#include <MMA8653.h>
#include <MPU6050.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))

#define TEMPERATURE_SENSOR(port)    (GetTemperatureSensorValue(port))
#define GYRO_SENSOR(port)    (GetGyroscopeValue(port))
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();

float GetTemperatureSensorValue(byte connector);
double GetGyroscopeValue(byte connector);

// 机器人设置处理
void artecRobotSetup();
// 机器人主要处理
void artecRobotMain();

// **********************************************************************
// 全局变量
// **********************************************************************
Studuino board;	// Studuino电路板目标
unsigned long StartTime;	// For timer

// **********************************************************************
// 程序
// **********************************************************************
