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

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

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

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

//#define LINE_BLACK //白地・黒ライン有効

const unsigned long LAP_COUNT = 848;
const unsigned long TARGET_NONE_COUNT = LAP_COUNT*90/848;

const int LANCE_ANGLE_PARALLEL1       =  90 -  53;  //度
const int LANCE_ANGLE_PARALLEL2       =  90 -  43;
const int LANCE_ANGLE_LEFT_VERTICAL   =  90 - 120;
const int LANCE_ANGLE_RIGHT_VERTICAL  =  90 -  57;

const byte SPEED_DEFAULT = 255;

const int HANDLE_DEGREE_DEFAULT = 90; //度
const unsigned int HANDLE_MIN = ( 800+30); //us
const unsigned int HANDLE_MAX = (2300+30); //us
const float HANDLE_KP = 4.0;

const int LANCE_DEGREE_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_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_HANDLE  = 10;
const byte PIN_SERVO_LANCE   =  9;

const byte PIN_MOTOR_LEFT_1  =  5;
const byte PIN_MOTOR_LEFT_2  =  7;
const byte PIN_MOTOR_RIGHT_1 =  6;
const byte PIN_MOTOR_RIGHT_2 =  8;

enum {
  MODE_STOP,                  //停止
  MODE_TRACE,                 //標準
  MODE_LEFT,                  //左カーブ
  MODE_TARGET_PARALLEL1,      //平行標的1
  MODE_TARGET_PARALLEL2,      //平行標的2
  MODE_TARGET_RIGHT_VERTICAL, //右垂直標的
  MODE_TARGET_LEFT_VERTICAL,  //左垂直標的
  MODE_TARGET_CYLINDER        //円筒標的
};

const byte MASK_MODE_TRACE         = 0b01111110;
const byte MASK_CHECK_MARKER       = 0b10000001;

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

Servo servoHandle;
Servo servoLance;
Motor motorR;
Motor motorL;

byte mode;
volatile unsigned long counter = 0;
unsigned long modeChangeCounter;

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

void traceLeft(byte sensor){
  switch(sensor&MASK_MODE_TRACE){
  case 0b01000000:
    handle(-5*HANDLE_KP);
    motorMode(GO, GO, 0, SPEED_DEFAULT);
    break;
  case 0b01100000:
    handle(-4*HANDLE_KP);
    motorMode(GO, GO, 0, SPEED_DEFAULT);
    break;
  case 0b00100000:
    handle(-3*HANDLE_KP);
    motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
    break;
  case 0b00110000:
    handle(-2*HANDLE_KP);
    motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
    break;
  case 0b00010000:
    handle(-2);
    motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
    break;
  case 0b00011000:
    handle(0);
    motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
    break;
  case 0b00001000:
    handle(2);
    motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
    break;
  case 0b00001100:
    handle(2*HANDLE_KP);
    motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
    break;
  case 0b00000100:
    handle(3*HANDLE_KP);
    motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
    break;
  case 0b00000110:
    handle(4*HANDLE_KP);
    motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
    break;
  case 0b00000010:
    handle(5*HANDLE_KP);
    motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
    break;
  }
}

void trace(byte sensor){
  switch(sensor&MASK_MODE_TRACE){
  case 0b01000000:
    handle(-5*HANDLE_KP);
    motorMode(GO, GO, 0, SPEED_DEFAULT*0.1);
    break;
  case 0b01100000:
    handle(-4*HANDLE_KP);
    motorMode(GO, GO, 0, SPEED_DEFAULT*0.1);
    break;
  case 0b00100000:
    handle(-3*HANDLE_KP);
    motorMode(GO, GO, SPEED_DEFAULT*0.333, SPEED_DEFAULT);
    break;
  case 0b00110000:
    handle(-2*HANDLE_KP);
    motorMode(GO, GO, SPEED_DEFAULT*0.667, SPEED_DEFAULT);
    break;
  case 0b00010000:
    handle(-2);
    motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
    break;
  case 0b00011000:
    handle(0);
    motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
    break;
  case 0b00001000:
    handle(2);
    motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
    break;
  case 0b00001100:
    handle(2*HANDLE_KP);
    motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
    break;
  case 0b00000100:
    handle(3*HANDLE_KP);
    motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
    break;
  case 0b00000110:
    handle(4*HANDLE_KP);
    motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
    break;
  case 0b00000010:
    handle(5*HANDLE_KP);
    motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
    break;
  }
}

void motorMode(byte leftMode, byte rightMode, byte leftSpeed, byte rightSpeed){
  motorL.mode( leftMode,  leftSpeed);
  motorR.mode(rightMode, rightSpeed);
}

void handle(int degree){
  servoHandle.write(HANDLE_DEGREE_DEFAULT + degree);
}

void lance(int degree){
  servoLance.write(LANCE_DEGREE_DEFAULT - degree);
}

int lanceRead(){
  return LANCE_DEGREE_DEFAULT - servoLance.read();
}

/* ロータリーエンコーダーの変化を見る
  todo:外部割り込み使ってみたい、、、
*/
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];
    counter++;
  }
}

int checkMarker(byte sensor){
  int mode = MODE_TRACE;
  switch(sensor & MASK_CHECK_MARKER){
  case 0b10000001:
    modeChangeCounter = counter;
    mode = MODE_LEFT;
    break;
  case 0b00000001:
    modeChangeCounter = counter;
    if(counter%LAP_COUNT > LAP_COUNT/2){
      mode = MODE_TARGET_RIGHT_VERTICAL;
    }else if(counter%LAP_COUNT < LAP_COUNT/7){
      mode = MODE_TARGET_PARALLEL1;
    }else if(counter%LAP_COUNT < LAP_COUNT/3){
      mode = MODE_TARGET_PARALLEL2;
    }else{
      mode = MODE_TARGET_RIGHT_VERTICAL;
    }
    break;
  case 0b10000000:
    modeChangeCounter = counter;
    mode = MODE_TARGET_LEFT_VERTICAL;
    break;
  }
  return mode;
}

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 sensor = 0;  //黒が1、白が0
  if(digitalRead(PIN_SENSOR_0)) sensor += 0b00000001;
  if(digitalRead(PIN_SENSOR_1)) sensor += 0b00000010;
  if(digitalRead(PIN_SENSOR_2)) sensor += 0b00000100;
  if(digitalRead(PIN_SENSOR_3)) sensor += 0b00001000;
  if(digitalRead(PIN_SENSOR_4)) sensor += 0b00010000;
  if(digitalRead(PIN_SENSOR_5)) sensor += 0b00100000;
  if(digitalRead(PIN_SENSOR_6)) sensor += 0b01000000;
  if(digitalRead(PIN_SENSOR_7)) sensor += 0b10000000;
  return sensor;
}

void setup() {
  //サーボ
  servoHandle.attach(PIN_SERVO_HANDLE, HANDLE_MIN, HANDLE_MAX);
  servoLance.attach(PIN_SERVO_LANCE, LANCE_MIN, LANCE_MAX);
  handle(0);
  lance(0);
  //モーター
  motorL.attach( PIN_MOTOR_LEFT_1,  PIN_MOTOR_LEFT_2);
  motorR.attach(PIN_MOTOR_RIGHT_1, PIN_MOTOR_RIGHT_2);
  motorL.mode(STOP);
  motorR.mode(STOP);
  //センサー
  sensorInit();
  //エンコーダー
  pinMode(PIN_ROT, INPUT);
  //LED
  pinMode(PIN_LED, OUTPUT);
  digitalWrite(PIN_LED, HIGH);
  //モード
  mode = MODE_TRACE;
  delay(1000);
  /* スタート */
  digitalWrite(PIN_LED, LOW);
  handle(0);
  lance(0);
  motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
}

void loop(){
  /* センサーを見る */
  byte sensor = 0;
  sensor = ~sensorRead(); //ライン(白)が1、地面(黒)が0
  count();  //ロータリーエンコーダーの変化を見る
  /* 全体の動作を決定する */
  switch(mode){
  case MODE_TARGET_RIGHT_VERTICAL:  //右垂直標的
    lance(LANCE_ANGLE_RIGHT_VERTICAL);
    mode = MODE_TRACE;
    break;
  case MODE_TARGET_LEFT_VERTICAL: //左垂直標的
    lance(LANCE_ANGLE_LEFT_VERTICAL);
    mode = MODE_TRACE;
    break;
  case MODE_TARGET_PARALLEL1: //平行標的1
    trace(sensor);
    lance(LANCE_ANGLE_PARALLEL1);
    if(counter - modeChangeCounter > 24){
      lance(LANCE_ANGLE_PARALLEL1+8);
      mode = MODE_TRACE;
    }
    break;
  case MODE_TARGET_PARALLEL2: //平行標的2
    trace(sensor);
    lance(LANCE_ANGLE_PARALLEL2);
    if(counter - modeChangeCounter > 26){
      lance(LANCE_ANGLE_PARALLEL2+8);
      mode = MODE_TRACE;
    }
    break;
  case MODE_TRACE:
    trace(sensor);
    mode = checkMarker(sensor);
    if(counter > LAP_COUNT*10){
      mode = MODE_STOP;
    }
    break;
  case MODE_LEFT: //左カーブ
    if(counter - modeChangeCounter < TARGET_NONE_COUNT/2) trace(sensor);
    else traceLeft(sensor);
//    trace(sensor);
    if(counter < LAP_COUNT) lance(30);  //1周目は円筒標的をねらいたい
    else lance(-30);
    if(counter - modeChangeCounter > TARGET_NONE_COUNT){
      lance(0);
      mode = MODE_TRACE;
    }
    break;
  case MODE_TARGET_CYLINDER:  //円筒標的
    digitalWrite(PIN_LED, HIGH);
    handle(0);
    lance(-90);
    motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
    if(sensor&MASK_CHECK_MARKER == 0b10000001){
      modeChangeCounter = counter;
    }else if( counter - modeChangeCounter > LAP_COUNT/21 ){
      mode = MODE_STOP;
    }
    break;
  case MODE_STOP: //停止
    digitalWrite(PIN_LED, HIGH);
    motorL.mode(STOP);
    motorR.mode(STOP);
    break;
  }
}