//ルンバ

#define NOTE_B0  31
#define NOTE_C1  33
#define NOTE_CS1 35
#define NOTE_D1  37
#define NOTE_DS1 39
#define NOTE_E1  41
#define NOTE_F1  44
#define NOTE_FS1 46
#define NOTE_G1  49
#define NOTE_GS1 52
#define NOTE_A1  55
#define NOTE_AS1 58
#define NOTE_B1  62
#define NOTE_C2  65
#define NOTE_CS2 69
#define NOTE_D2  73
#define NOTE_DS2 78
#define NOTE_E2  82
#define NOTE_F2  87
#define NOTE_FS2 93
#define NOTE_G2  98
#define NOTE_GS2 104
#define NOTE_A2  110
#define NOTE_AS2 117
#define NOTE_B2  123
#define NOTE_C3  131
#define NOTE_CS3 139
#define NOTE_D3  147
#define NOTE_DS3 156
#define NOTE_E3  165
#define NOTE_F3  175
#define NOTE_FS3 185
#define NOTE_G3  196
#define NOTE_GS3 208
#define NOTE_A3  220
#define NOTE_AS3 233
#define NOTE_B3  247
#define NOTE_C4  262
#define NOTE_CS4 277
#define NOTE_D4  294
#define NOTE_DS4 311
#define NOTE_E4  330
#define NOTE_F4  349
#define NOTE_FS4 370
#define NOTE_G4  392
#define NOTE_GS4 415
#define NOTE_A4  440
#define NOTE_AS4 466
#define NOTE_B4  494
#define NOTE_C5  523
#define NOTE_CS5 554
#define NOTE_D5  587
#define NOTE_DS5 622
#define NOTE_E5  659
#define NOTE_F5  698
#define NOTE_FS5 740
#define NOTE_G5  784
#define NOTE_GS5 831
#define NOTE_A5  880
#define NOTE_AS5 932
#define NOTE_B5  988
#define NOTE_C6  1047
#define NOTE_CS6 1109
#define NOTE_D6  1175
#define NOTE_DS6 1245
#define NOTE_E6  1319
#define NOTE_F6  1397
#define NOTE_FS6 1480
#define NOTE_G6  1568
#define NOTE_GS6 1661
#define NOTE_A6  1760
#define NOTE_AS6 1865
#define NOTE_B6  1976
#define NOTE_C7  2093
#define NOTE_CS7 2217
#define NOTE_D7  2349
#define NOTE_DS7 2489
#define NOTE_E7  2637
#define NOTE_F7  2794
#define NOTE_FS7 2960
#define NOTE_G7  3136
#define NOTE_GS7 3322
#define NOTE_A7  3520
#define NOTE_AS7 3729
#define NOTE_B7  3951
#define NOTE_C8  4186
#define NOTE_CS8 4435
#define NOTE_D8  4699
#define NOTE_DS8 4978

#define MOVE_S 0
#define MOVE_F 1
#define MOVE_L 2
#define MOVE_R 3
#define MOVE_B 4
#define MOVE_BL 5
#define MOVE_BLN 7
#define MOVE_RECO 6
#define MOVE_N 99

#include <SparkFun_TB6612.h>
#include <CapacitiveSensor.h>
#include <Wire.h>
#include <VL53L1X.h>
#include <VarSpeedServo.h> 

VL53L1X sensor;

VarSpeedServo myServo;  // create a servo object

#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 32 // OLED display height, in pixels

CapacitiveSensor   cs_4_2 = CapacitiveSensor(14,15);        // 10M resistor between pins 4 & 2, pin 2 is sensor pin, add a wire and or foil if desired

//ここまでWAV

#define OLED_RESET     4 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
//A5とA4はディスプレイ用　あとVCC5V

#define LED 13    //kiban
#define L_LED 7   //analog
#define R_LED 3   //analog
#define RESET 2   //digital

const int kando = 730;

const int motorA1 = 8;  // IN1
const int motorA2 = 7;  // IN2
const int motorAP = 6;  // IN2
const int motorB1 = 10;  // IN1
const int motorB2 = 12;  // IN2
const int motorBP = 11;  // IN2
const int motorST = 9;  // IN2

const int BZ = 3;  // IN2
const int offsetA = 1;
const int offsetB = 1;

Motor motor1 = Motor(motorA1, motorA2, motorAP, offsetA, motorST);
Motor motor2 = Motor(motorB1, motorB2, motorBP, offsetB, motorST);

unsigned long limitTime = 300000;// * 50;//0 * 1;
int moveSpeed = 80;

bool resetFlag1L;
bool resetFlag1F;

unsigned long startLimit;
unsigned long randomTime;
unsigned long  startTailTime;
unsigned long  startTime;
int thisNoteA;
float maxSpeed;
  float flatOld, flonOld;
float speedOld;
float speedOld2;
unsigned long st;
bool flag;
bool blinkFlag;
int modeCount;
bool offlineFlag;
bool mapFlag;
float flat, flon;
bool rpmFlag;
// 帝国のマーチ A メロ
int melody[] = {
  NOTE_G3, NOTE_G3, NOTE_G3, NOTE_DS3, NOTE_AS3, NOTE_G3, NOTE_DS3, NOTE_AS3, NOTE_G3,
  NOTE_D4, NOTE_D4, NOTE_D4, NOTE_DS4, NOTE_AS3, NOTE_FS3, NOTE_DS3, NOTE_AS3, NOTE_G3
};
bool randomFlag;
bool tailFlag;
bool light;

int type;
int timeCount;
int stackCount;

float kakudo;
int forwardSpeed = 160;

int oldx;
int oldy;

// 音の長さ 2, 4, 8 はそれぞれ 2分音符、4分音符、8分音符
int noteDurations[] = {
  4, 4, 4, 6, 8, 4, 6, 8, 2,
  4, 4, 4, 6, 8, 4, 6, 8, 2
  
};

void setup()
{
//delay(100) ;

  tone(BZ, NOTE_CS8, 30);

  pinMode(LED, OUTPUT);
  pinMode(L_LED, INPUT); 
  pinMode(R_LED, INPUT); 
  pinMode(RESET, INPUT_PULLUP);

//ここからモータ
  myServo.attach(5); // attaches the servo on pin 9 to the servo object しっぽ
  myServo.write(90);

//ここから文字表示
  Serial.begin(9600);
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3C for 128x32
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }
  display.display();
  //delay(2000); // Pause for 2 seconds

  // Clear the buffer
  display.clearDisplay();
  testfillcircle();
  // Draw a single pixel in white
  //display.drawPixel(10, 10, SSD1306_WHITE);

  display.display();
    digitalWrite(LED, HIGH);
    delay(500);
    digitalWrite(LED, LOW);

  sensorSetup();

  //Serial.begin(115200);
  offlineFlag= true;
  cs_4_2.set_CS_AutocaL_Millis(0xFFFFFFFF);     // turn off autocalibrate on channel 1 - just as an example
}
void testfillcircle(void) {
  display.clearDisplay();

  for(int16_t i=max(display.width(),display.height())/2; i>0; i-=3) {
    // The INVERSE color is used so circles alternate white/black
    display.fillCircle(display.width() / 2, display.height() / 2, i, SSD1306_INVERSE);
    display.display(); // Update screen with each newly-drawn circle
    delay(1);
  }

  delay(5);
}

void NormalMode()
{
  float laser = sensor.read();
  display.print("laser ");
  display.print(laser);
  if (sensor.timeoutOccurred()) { 
    display.print(" TIMEOUT");
    tone(BZ, NOTE_DS8, 1300);
    laser=1000; 
  }
  
  //float fs = analogRead(L_LED);
  display.print(" S:");
  display.println(myServo.read());//フォトリフレクタ
  //減速
  if(laser < 600){
    forwardSpeed = 100;
    if(laser >= 400){
      if(kakudo < 90 -20){
        if(Move(MOVE_L, 5)){
          tone(BZ, NOTE_DS6, 100);
        }
      }
      else if(kakudo > 90 +20){
        if(Move(MOVE_R, 5)){
          tone(BZ, NOTE_DS6, 100);
        }
      }
    }
  }
  else{
    //最大速度
    forwardSpeed = 190;
  }
  if(laser < 400 ||  digitalRead(RESET) == LOW){//fs < kando ||
    if(Move(MOVE_BLN, 20)){
      tone(BZ, NOTE_DS6, 300);
      startTime = millis();
    }
  }
  else{
    Move(MOVE_F, 0);
  }
}

void CommonMode()
{
  //ヒットしてからの時間経過 10秒
  if(millis() -startTime > 20000){
    startTime = millis();
    tone(BZ, NOTE_DS8, 300);
    delay(300);
    tone(BZ, NOTE_DS8, 300);
    Move(MOVE_RECO, 100);
    stackCount = 0;
  }
  TailMode();
}

void TailMode()
{
    if(tailFlag){
      myServo.write(90 -30, 40);
      //kakudo += -10;
      //if(kakudo < -70){
      //  kakudo = -70;
      //}
      if(kakudo < 90 -25){
        tailFlag = !tailFlag;
      }
    }
    else{
      myServo.write(90 +30, 40);
      //kakudo += 10;
      //if(kakudo > 70){
      //  kakudo = 70;
      //}
      if(kakudo > 90 +25){
        tailFlag = !tailFlag;
      }
    }
    kakudo = myServo.read();
}

void MapTailMode()
{
    if(tailFlag){
      myServo.write(90 -40, 20);//70
      kakudo += -10;
      if(kakudo < -70){
        kakudo = -70;
      }
    }
    else{
      myServo.write(90 +40, 20);
      kakudo += 10;
      if(kakudo > 70){
        kakudo = 70;
      }
    }
    kakudo = myServo.read();
    if(millis() -startTailTime > 1000 && !myServo.isMoving()){
      startTailTime = millis();
      display.clearDisplay();
      tailFlag = !tailFlag;
      if(tailFlag){
        //kakudo = 70;
      }
      else{
        //kakudo = -70;
      }
    }
}

void loop()
{
  long total1 = 0;
  total1 = cs_4_2.capacitiveSensor(30);
  display.print(total1);
  
  if(offlineFlag){
    motor1.drive(0);
    motor2.drive(0);

    if(!mapFlag){
      MapTailMode();
      if(kakudo == 70){
        //display.clearDisplay();
      }
      display.setCursor(0,0);             // Start at top-left corner
      display.setTextColor(SSD1306_WHITE);
      display.setTextSize(1);             // Draw 2X-scale text
      //display.println("MAP");
      float laser = sensor.read();
      //display.print(laser);
      float range = laser/20;//5
      if(range > 40){
        range = 40;
      }
      float r = kakudo*3.1415/180;
      //display.println(r);
      //display.println(kakudo);
      //display.drawPixel(SCREEN_WIDTH/2 +(cos(r) * range), 0 +(sin(r) * range), SSD1306_WHITE);//SCREEN_HEIGHT/2
      display.drawLine(SCREEN_WIDTH/2 +(cos(r) * range), 0 +(sin(r) * range), oldx, oldy, SSD1306_WHITE);//SCREEN_HEIGHT/2
      oldx = SCREEN_WIDTH/2 +(cos(r) * range);
      oldy = 0 +(sin(r) * range);
      delay(1);
    }
    else{
      display.clearDisplay();
      
      display.setCursor(0,0);             // Start at top-left corner
      display.setTextColor(SSD1306_WHITE);
      display.setTextSize(1);             // Draw 2X-scale text
  
      display.println("STANDBY");
      int noteDuration = 1000 / noteDurations[thisNoteA];
      tone(BZ, melody[thisNoteA], noteDuration);
    
      int pauseBetweenNotes = noteDuration * 1.30;
      delay(pauseBetweenNotes);
      //noTone(BZ);
      thisNoteA++;
      if(thisNoteA >= 18){// sizeof(noteDurations) / sizeof(noteDurations[0])){
        thisNoteA = 0;
      }
    }
  }
  else{
    display.clearDisplay();
    
    display.setCursor(0,0);             // Start at top-left corner
    display.setTextColor(SSD1306_WHITE);
    display.setTextSize(1);             // Draw 2X-scale text
    //if(randomFlag){
    //  RandomMode();
    //}
    //else{
      NormalMode();
    //}
    CommonMode();

    MoveAction();
    //digitalWrite( motorST, HIGH );
  
    //タイムリミット
    display.println((startLimit -millis()) +limitTime);
    if(limitTime +startLimit < millis()){
      offlineFlag = true;
    }
    delay(5);
  }

  display.display();

  if (total1 > 200){//digitalRead(RESET) == LOW || 
    flag = false;
    maxSpeed = 0;
    flatOld = flat;
    flonOld = flon;
    type = MOVE_N;

    startTime = millis();
    startLimit = millis();
    tone(BZ, NOTE_DS8, 300);
    digitalWrite(LED, HIGH);
    delay(200);
    digitalWrite(LED, LOW);
    delay(200);
    digitalWrite(LED, HIGH);
    delay(200);
    digitalWrite(LED, LOW);
    delay(200);
    digitalWrite(LED, HIGH);
    delay(200);
    digitalWrite(LED, LOW);

    display.clearDisplay();
  
    //display.drawPixel(10, 10, SSD1306_WHITE);
  
    display.setCursor(0,0);             // Start at top-left corner
    offlineFlag = !offlineFlag;

    display.drawLine(0, display.height() -2, display.width(), display.height() -2, SSD1306_WHITE);
    display.display();
    delay(1000);
    modeCount++;
  }
  else{
    modeCount = 0;
  }
}

bool Move(int t, float c)
{
  if(timeCount <= 0){// || (type == t && c > 0)
    type = t;
    timeCount = c;
    return true; 
  }
  return false;
}

void MoveAction()
{
  switch(type){
    case MOVE_S:
      display.print("STOP");
      digitalWrite( motorA1, LOW );
      digitalWrite( motorA2, LOW );
      digitalWrite( motorB1, LOW );
      digitalWrite( motorB2, LOW );
      break;
    case MOVE_F:
      display.print("F");
      //motor1.drive(moveSpeed,5);
      //motor2.drive(moveSpeed,5);
      forward(motor1, motor2, forwardSpeed);//moveSpeed*1.5
      /*digitalWrite( motorA1, HIGH );
      digitalWrite( motorA2, LOW );
      digitalWrite( motorB1, HIGH );
      digitalWrite( motorB2, LOW );*/
      break;
    case MOVE_L://左側反応の時
      display.print("L");
      motor1.drive(moveSpeed, 50);
      motor2.drive(moveSpeed/3, 50);
      break;
    case MOVE_R://右側反応の時
      display.print("R");
      motor1.drive(moveSpeed/3, 50);
      motor2.drive(moveSpeed, 50);

      break;
    case MOVE_RECO:
      display.print("RECO");
      if(timeCount > 75)
      {
        motor1.drive(-moveSpeed,5);
        motor2.drive(-moveSpeed,5);
      }
      else if(timeCount > 50)
      {
        motor1.drive(-255,5);
        motor2.drive(255,5);
      }
      else if(timeCount > 25)
      {
        motor1.drive(-moveSpeed,5);
        motor2.drive(-moveSpeed,5);
      }
      else {
        motor1.drive(moveSpeed,5);
        motor2.drive(-moveSpeed,5);
      }
      break;
      
    case MOVE_B:
      display.print("B");
      motor1.drive(-moveSpeed,5);
      motor2.drive(-moveSpeed,5);
      break;
    case MOVE_BL://50ms
      //back(motor1, motor2, 50);
      if(timeCount > 45)
      {
        display.print("BL1");
        motor1.brake();
        motor2.brake();
      }
      else if(timeCount > 25)
      {
        display.print("BL1");
        motor1.drive(-moveSpeed);
        motor2.drive(-moveSpeed);
      }
      else
      {
        display.print("BL2");
        motor1.drive(-moveSpeed);
        motor2.drive(moveSpeed);
      }
      break;
    case MOVE_BLN://20
      //back(motor1, motor2, 50);
      if(timeCount > 18)//20-18
      {
        display.print("BLN1");
        motor1.drive(-moveSpeed/4,5);
        motor2.drive(-moveSpeed/4,5);
      }
      else if(timeCount > 10)//18-10
      {
        display.print("BLN2");
        motor1.drive(-moveSpeed/1.5,5);
        motor2.drive(-moveSpeed/1.5,5);
      }
      else
      {
        display.print("BLN3");
        motor1.drive(-moveSpeed/3,5);
        motor2.drive(moveSpeed*1.5,5);//(70 -kakudo
      }
      break;
  }
  display.print(" ");
  timeCount--;
}

void loopLed(float speed)
{

  float nowSpeed = (speed +speedOld2)/2;
  //ブレーキランプ
  if(speedOld < nowSpeed -0.8){
  }
  if(speed > 2.2 || offlineFlag){
    if(blinkFlag){
      if(offlineFlag){
        digitalWrite(LED, LOW);
      }
      blinkFlag = false;
    }
    else{
      if(offlineFlag){
        digitalWrite(LED, HIGH);
      }
      blinkFlag = true;
    }
  }
  else{
    if(!blinkFlag){
      blinkFlag = true;
    }
    
  }
  speedOld = (speedOld2 +speedOld)/2;
  speedOld2 = speed;
}

void sensorSetup()
{
  Serial.begin(115200);
  Wire.begin();
  Wire.setClock(400000); // use 400 kHz I2C

  sensor.setTimeout(500);
  if (!sensor.init())
  {
    Serial.println("Failed to detect and initialize sensor!");
    while (1);
  }
  
  sensor.setDistanceMode(VL53L1X::Long);
  sensor.setMeasurementTimingBudget(50000);

  // Start continuous readings at a rate of one measurement every 50 ms (the
  // inter-measurement period). This period should be at least as long as the
  // timing budget.
  sensor.startContinuous(50);
  sensor.setTimeout(0);
}