/*
 * STUDUINO
 * (C) 2014 Artec Corporation
 * 
 * 본 프로그램을 Arduino IDE에서 실행할 경우, Studuino 라이브러리를 설치해야 합니다.
 * 
 * 
 * Studuino 라이브러리의 설치 방법은 아래의 홈페이지를 참조해 주십시오.
 * 
 * http://www.artec-kk.co.jp/studuino
 * 
 * 
 * --------------------------------------
 *  ■ artecRobotSetup함수：접속파츠등의 초기화 함수 입니다.
 *  ■ artecRobotMain함수 ：「제어 시작」블록에 접속한 프로그램 입니다.
 *  ■ ARSR_*함수         ：「함수」블록에 작성한 프로그램 입니다.
 * --------------------------------------
 */
#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)     // 블록 프로그래밍 환경의 센서 최소치
#define BMAX    (100)   // 블록 프로그래밍 환경의 센서 최대치
#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))
#define ULTRASONIC_SENSOR()    (GetUltrasonicSensorValue())

#define DWEIGHT	(1.818)

#define MINMAX(v, mv, Mv)	(min(max(v,mv),Mv))

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);
float GetUltrasonicSensorValue();

// 로봇 셋업 처리
void artecRobotSetup();
// 로봇 메인 처리
void artecRobotMain();

// **********************************************************************
// 글로벌 변수
// **********************************************************************
Studuino board;	// Studuino 기판 오브젝트
unsigned long StartTime;	// For timer

bool IRRemoteUsed = false;	// 赤外線受信センサ使用中フラグ
bool BeepOn = false;
bool DCMotorOn = false;
// **********************************************************************
// 프로그램
// **********************************************************************

