/*
競技用ランサーロボット
半蔵 2.1 - PR用

[7 654321 0]
  |      |
  |      |
[]---{}---[]
    / .\
   /  | \
[]+--{}--+[]

(C)2013 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt]
*/

#include <Servo.h>
#include "Motor.h"
#include "pitches.h"

void disk0(){
   /*
   $pu1
   l12　o5
   c4f4b-4agfg8.g16
   <c4e-4dc>b-<c4>a4b-4
   f8.d16
   $pu2
   f2.
   */

  int melodyDisk0[] = {
    NOTE_C4, NOTE_F4, NOTE_AS4, NOTE_A4, NOTE_G4, NOTE_F4, NOTE_G4,
    NOTE_C5, NOTE_DS5,NOTE_D5, NOTE_C5,NOTE_AS4,
    NOTE_C5,NOTE_A4,NOTE_AS4,
    NOTE_F4,NOTE_D4,NOTE_F4
  };

  // note durations: 4 = quarter note, 8 = eighth note, etc.:
  int noteDurationsDisk0[] = {
    4,4,4,11,11,11,4,
    4,4,11,11,11,
    4,4,4,
    6,11,2
  };

  for (int thisNote = 0; thisNote < 18; thisNote++) {
    int noteDuration = 1300/noteDurationsDisk0[thisNote];
    tone(3, melodyDisk0[thisNote],noteDuration);
    int pauseBetweenNotes = noteDuration * 1.30;
    delay(pauseBetweenNotes);
    noTone(3);
  }
}

void disk1(){
  int melodyDisk1[] = {NOTE_B5,NOTE_D6,NOTE_G6};
  for (int thisNote = 0; thisNote < 3; thisNote++) {
    tone(3, melodyDisk1[thisNote]);
    delay(100);
    noTone(3);
  }
}



const unsigned long LAP_DISTANCE  = 500;  //ロータリーエンコーダーのカウント数/周
const unsigned long TURN_DISTANCE = 50;  //カーブ中でのカウント数

const int LANCE_ANGLE_PARALLEL_1 =  90 -  62;  //度
const int LANCE_ANGLE_PARALLEL_2 =  90 -  50;  //度
const int LANCE_ANGLE_VERTICAL_L =  90 - 120;  //度
const int LANCE_ANGLE_VERTICAL_R =  90 -  62;  //度

const byte SPEED_DEFAULT = 0xff;

//Servo  : SC-0352
// 60deg : 1265 us
// 90deg : 1535 us
//120deg : 1805 us
const int STEERING_DEFAULT = 90; //度
const unsigned int STEERING_MIN = ( 800+30); //us
const unsigned int STEERING_MAX = (2300+30); //us

const int LANCE_DEFAULT = 90; //度
const unsigned int LANCE_MIN = ( 544-62); //us
const unsigned int LANCE_MAX = (2400-62); //us

const byte PIN_LED     = 4;
const byte PIN_BUZZER  = 3;
const byte PIN_ROT     = 2;

const byte PIN_SENSOR_0 = 12;  //右端
const byte PIN_SENSOR_1 = 13;
const byte PIN_SENSOR_2 = 14;
const byte PIN_SENSOR_3 = 15;
const byte PIN_SENSOR_4 = 16;
const byte PIN_SENSOR_5 = 17;
const byte PIN_SENSOR_6 = 18;
const byte PIN_SENSOR_7 = 19;  //左端

const byte PIN_SERVO_STEERING  = 10;
const byte PIN_SERVO_LANCE     =  9;

const byte MASK_LINE    = 0b01111110;
const byte MASK_MARKER  = 0b10000001;

/**************************************/

Servo steeringServo;
Servo lanceServo;
Motor motorR;
Motor motorL;

//ラインが1、地面が0、LSBが左端、MSBが右端
byte sensor = 0x00;  

volatile unsigned long distance = 0;
unsigned long modeChangedDistance = 0;

/**************************************/

enum mode_t {
  MODE_STOP,      //停止
  MODE_STRAIGHT,  //まっすぐ進む
  MODE_TURN,      //曲がる
  MODE_TURN_LEFT, //左に曲がる
  MODE_TURN_RIGHT,//右に曲がる
  MODE_ATTACK_PARALLEL_1,  //平行標的1
  MODE_ATTACK_PARALLEL_2,  //平行標的2
  MODE_ATTACK_VERTICAL_R,  //右垂直標的
  MODE_ATTACK_VERTICAL_L,  //左垂直標的
  MODE_ATTACK_CYLINDER     //円筒標的
} 
mode;

/**************************************/

/* センサーを読んで、車体の動作を決定する */
void trace(){
  switch(sensor&MASK_LINE){
  case 0b01000000:
    steering(-20);
    motorL.speed(SPEED_DEFAULT/2);
    motorR.speed(SPEED_DEFAULT);
    break;
  case 0b01100000:
    steering(-15);
    motorL.speed(SPEED_DEFAULT/2);
    motorR.speed(SPEED_DEFAULT);
    break;
  case 0b00100000:
    steering(-11);
    motorL.speed(SPEED_DEFAULT);
    motorR.speed(SPEED_DEFAULT);
    break;
  case 0b00110000:
    steering(-8);
    motorL.speed(SPEED_DEFAULT);
    motorR.speed(SPEED_DEFAULT);
    break;
  case 0b00010000:
    steering(-4);
    motorL.speed(SPEED_DEFAULT);
    motorR.speed(SPEED_DEFAULT);
    break;
  case 0b00011000:
    steering(0);
    motorL.speed(SPEED_DEFAULT);
    motorR.speed(SPEED_DEFAULT);
    break;
  case 0b00001000:
    steering(4);
    motorL.speed(SPEED_DEFAULT);
    motorR.speed(SPEED_DEFAULT);
    break;
  case 0b00001100:
    steering(8);
    motorL.speed(SPEED_DEFAULT);
    motorR.speed(SPEED_DEFAULT);
    break;
  case 0b00000100:
    steering(11);
    motorL.speed(SPEED_DEFAULT);
    motorR.speed(SPEED_DEFAULT);
    break;
  case 0b00000110:
    steering(15);
    motorL.speed(SPEED_DEFAULT);
    motorR.speed(SPEED_DEFAULT);
    break;
  case 0b00000010:
    steering(20);
    motorL.speed(SPEED_DEFAULT);
    motorR.speed(SPEED_DEFAULT);
    break;
  }
}

/* ステアリングを中央からa[度]だけ動かす
  <- - 0 + ->
   |      |
   |      |
 []---{}---[]
     / .\
*/
void steering(int a){
  steeringServo.write(STEERING_DEFAULT + a);
}

/* ランスを中央からa[度]だけ動かす
  <- - 0 + ->
     / .\
    /  | \
 []+--{}--+[]
*/
void lance(int a){
  lanceServo.write(LANCE_DEFAULT - a);
}

/* センサーのアレイの初期化 */
void sensorInit(){
  pinMode(PIN_SENSOR_0, INPUT);  //右端
  pinMode(PIN_SENSOR_1, INPUT);
  pinMode(PIN_SENSOR_2, INPUT);
  pinMode(PIN_SENSOR_3, INPUT);
  pinMode(PIN_SENSOR_4, INPUT);
  pinMode(PIN_SENSOR_5, INPUT);
  pinMode(PIN_SENSOR_6, INPUT);
  pinMode(PIN_SENSOR_7, INPUT);  //左端
}

/* センサーアレイの状態を見る*/
byte sensorRead(){
  byte b=0;  //黒が1、白が0
//  if(analogRead(PIN_SENSOR_0)>511) b += 0b00000001; //右端
//  if(analogRead(PIN_SENSOR_1)>511) b += 0b00000010;
  if(analogRead(PIN_SENSOR_2)>511) b += 0b00000100;
  if(analogRead(PIN_SENSOR_3)>511) b += 0b00001000;
  if(analogRead(PIN_SENSOR_4)>511) b += 0b00010000;
  if(analogRead(PIN_SENSOR_5)>511) b += 0b00100000;
  if(analogRead(PIN_SENSOR_6)>511) b += 0b01000000;
  if(analogRead(PIN_SENSOR_7)>511) b += 0b10000000;  //左端
  return sensor = b;
}

void modeSet(){
  static int i = 0;
  if(sensor == 0){
    i++;
  }else i=0;
  if(i>0){
    mode = MODE_TURN_LEFT;
    return;
  }
  
  switch(sensor&MASK_MARKER){
//  default:
//  case 0b00000000:
//    mode = MODE_STRAIGHT;
//    break;
//  case 0b10000001:
//    modeChangedDistance = distance;
//    mode = MODE_TURN;
//    break;
  case 0b00000001:
    modeChangedDistance = distance;
    mode = MODE_ATTACK_PARALLEL_1;
    break;
  case 0b10000000:
    modeChangedDistance = distance;
    mode = MODE_ATTACK_VERTICAL_L;
    break;
  }
}

/* ロータリーエンコーダーの変化を見る */
void count(){
  static byte rot[2] = {0}; //rot[0]:今の状態, rot[1]:前回の状態
  rot[0] = digitalRead(PIN_ROT);
  if(rot[0] != rot[1]){
    rot[1] = rot[0];
    distance++;
  }
}

void startBeep(){
  tone(PIN_BUZZER,2000);
  delay(100);
  tone(PIN_BUZZER,1000);
  delay(100);
  noTone(PIN_BUZZER);
}

void setup(){
  //サーボ
  steeringServo.attach(PIN_SERVO_STEERING, STEERING_MIN, STEERING_MAX);
  lanceServo.attach(PIN_SERVO_LANCE, LANCE_MIN, LANCE_MAX);
  steering(0);
  lance(0);
  //モーター
  motorL.attach(5,7);
  motorR.attach(6,8);
  motorL.mode(STOP);
  motorR.mode(STOP);
  //センサー
  sensorInit();
  //エンコーダー
  pinMode(PIN_ROT, INPUT);
  //ブザー
  pinMode(PIN_BUZZER, OUTPUT);
  //LED
  pinMode(PIN_LED, OUTPUT);
  digitalWrite(PIN_LED, HIGH);
  /* 停止 */
  motorL.mode(STOP);
  motorR.mode(STOP);
  delay(1000);
  disk1();
  delay(1000);
  /* スタート */
  digitalWrite(PIN_LED, LOW);
  mode = MODE_STRAIGHT;
//  Serial.begin(9600);
//  while(1){
//    count();
//    Serial.println(distance);
//  }
}

void loop(){
  count();
  if(distance > 500){
    mode = MODE_STOP;
  }
  sensorRead();
  switch(mode){
  default:
  case MODE_STOP:      //停止
    motorL.mode(STOP);
    motorR.mode(STOP);
    digitalWrite(PIN_LED, HIGH);
    delay(1000);
    lance(0);
    steering(0);
    delay(500);
    disk0();
//    delay(1000);
//    tone(PIN_BUZZER,2000);
//    delay(500);
//    noTone(PIN_BUZZER);
    while(1){
      ;
    };
    break;
  case MODE_STRAIGHT:  //まっすぐ進む
    trace();
    modeSet();
    break;
  case MODE_TURN_LEFT: //左に曲がる
    lance(-70);
    if(distance - modeChangedDistance < TURN_DISTANCE){
      steering(-30);
      motorL.speed(SPEED_DEFAULT/2);
      motorR.speed(SPEED_DEFAULT);
    }else mode = MODE_STOP;
    break;
  case MODE_TURN_RIGHT://右に曲がる
    lance(0);
    if(distance - modeChangedDistance < TURN_DISTANCE){
      steering(30);
      motorL.speed(SPEED_DEFAULT);
      motorR.speed(SPEED_DEFAULT/2);
    }else mode = MODE_STOP;
    break;
  case MODE_ATTACK_PARALLEL_1:  //平行標的1
    trace();
    lance(LANCE_ANGLE_PARALLEL_1);
    if(distance - modeChangedDistance > 255){ //マーカーから少し進んで、叩く
      lance(LANCE_ANGLE_PARALLEL_1 + 10);
      mode = MODE_STRAIGHT;
    }
    break;
  case MODE_ATTACK_PARALLEL_2:  //平行標的2
    trace();
    lance(LANCE_ANGLE_PARALLEL_2);
    if(distance - modeChangedDistance > 283){ //マーカーから少し進んで、叩く
      lance(LANCE_ANGLE_PARALLEL_2 + 10);
      mode = MODE_STRAIGHT;
    }
    break;
  case MODE_ATTACK_VERTICAL_R:  //右垂直標
    trace();
    lance(LANCE_ANGLE_VERTICAL_R);
    if(distance - modeChangedDistance > 300){ //マーカーから少し進んで、まっすぐに戻す
      lance(0);
      mode = MODE_STRAIGHT;
    }
    break;
  case MODE_ATTACK_VERTICAL_L:  //左垂直標
    trace();
    lance(LANCE_ANGLE_VERTICAL_L);
    if(distance - modeChangedDistance > 5){ //マーカーから少し進んで、まっすぐに戻す
      lance(0);
      mode = MODE_STRAIGHT;
    }
    break;
  case MODE_ATTACK_CYLINDER:    //円筒標的
    trace();
    mode = MODE_STRAIGHT;
    break;
  }
}