#include <pcmConfig.h>
#include <TMRpcm.h>
#include <pcmRF.h>
#include <pcmConfig.h>
#include <SoftwareSerial.h>
#include <SD.h>                
#include "MotorDriver.h"

//pin setting
#define SD_ChipSelectPin 53  
#define LED_Pin 41
#define Speaker_Pin1 6
#define Speaker_Pin2 7
#define rx_Pin 31
#define tx_Pin 30

//for blth_serial
char letter_cson = 'q';
char letter_csoff = 'w';
char letter_EXStart = 's';
char letter_EXEnd = 'e';

//for test setting
unsigned long time = 0;
long hab = -1; //habituation time
int hstate = 0;
int test = 0;
int prob = 0;
int isi_a = -1;
int isi_b = 0; //interstimulus end
long int isi = 0;
int prob_a = 1;
int prob_b = 3;
int trial_set = -1; // how many trial do?
int speed1 = -1;
long us_start = -1;
long us_work = -1;
char *music = 0;
int trial = 0;
long start_direction;
long ran1;
//for question
char person_state = 0;
char test_state = 0;
char start_state = 0;
char default_state = 0;
char rotate_state = 0;

int HW_Mode = 0;
int time_stamp = 1; //

unsigned long starttime;
String serial_input=""; //

SoftwareSerial BT(rx_Pin, tx_Pin);
TMRpcm tmrpcm; //music play variable

int time_wait(long wait_time, int trial_set, int trial = 0, int time_stamp = 0);
void equip_check(int sd_test = 1 , int sound_test = 1, int motor_test = 1, int led_test = 1, int io_test = 1);
void break_test();
void select_rotate(long start_direction, int rotate_state, int trial);
void motor_move(int rotate_direction);

void setup(){
  //Pin Setup
  motordriver.init();

  Serial.begin(115200);
  Serial1.begin(115200);
  Serial2.begin(115200);
  BT.begin(115200);

  tmrpcm.speakerPin = Speaker_Pin1; 
  pinMode(Speaker_Pin2, OUTPUT);  //Pin pairs: 9,10 Mega: 5-2,6-7,11-12,46-45
  pinMode(Speaker_Pin1, OUTPUT);
  pinMode(LED_Pin, OUTPUT);

   if (!SD.begin(SD_ChipSelectPin)) { 
  Serial.println("No Sdcard");
  //return; 
  }
  else Serial.println("Yes SDcard");
}

void loop(){
  int x, delay_en;

  if(person_state == 0){
    Serial1.println("Who are you?");
    Serial1.println("a. HW   d. Debug   e. Equip_check");
  }
  
  while(person_state != 1) {
    while(Serial1.available()) {
      char val = (char) Serial1.read();
      if(val != -1) {
        switch(val) {
          case 'a':
            Serial1.println("Hyewon");
            speed1 = 90;
            hab = 180000;
            isi_a = 90;
            isi_b = 90;
            prob_a = 1;
            prob_b = 1;
            trial_set = 9;
            us_start = 7000;
            us_work = 3000;
            music = "sin1.wav";
            HW_Mode = 1;
            ran1 = millis();

            
            person_state = 1;
            break;
            
          case 'd':
            test_state = 1;
            default_state = 1;
            person_state = 1;

            break;

          case 'e':
            Serial1.println("Equip Check");
            equip_check();
            break;

          case 't':
            Serial1.println("Test isn't ongoing");
            break;
        }
        
        val = -1;
      }
    }
  }
  if(test_state == 0){
    Serial1.println("Which condition do you want?");
    Serial1.println("a. Conditioning   b. Habituation   c. Test   e. Extinction");
  }


  while(test_state != 1) {
    while(Serial1.available()) {
      char val = (char) Serial1.read();
      if(val != -1) {
        switch(val) {
          case 'a':
          Serial1.println("Conditioning");
            break;
  
          case 'b':
            Serial1.println("Habituation");
            speed1 = 0;
            trial_set = 30;

            if(HW_Mode == 1){
              speed1 = 0;
              trial_set = 4;
              isi_a = 40;
              isi_b = 40;
            }
            break;
  
          case 'c':
            Serial1.println("Test");
            speed1 = 0;
            trial_set = 1;
            us_start = 60000;
            us_work = 1;

            if(HW_Mode == 1){
              Serial1.println("HW_Mode");
              speed1 = 0;
              trial_set = 4;            
              isi_a = 60;
              isi_b = 60; 
              }
            break;

          case 'e':
            Serial1.println("Extinction");
            speed1 = 0;
            trial_set = 30;

            if(HW_Mode == 1){
              speed1 = 0;
              trial_set = 20;
              isi_a = 60;
              isi_b = 60;
            }
            break;  
            
        }
      }
      test_state = 1;
      val = -1;
    }
  }

  if(default_state == 1){
    Serial1.println("Do NOT type in before the question completes");
    Serial1.println("Robot speed?");
    
    while(speed1 == -1){
      while(Serial1.available()){
        serial_input = Serial1.readStringUntil('\n');
        if(serial_input != -1){
          speed1 = serial_input.toInt();
          Serial1.println(speed1);
        }
      }
    }
    
    Serial1.println("Habituation time?");
    while(hab == -1){
      while(Serial1.available()){
        serial_input = Serial1.readStringUntil('\n');
        if(serial_input != -1){
          hab = (serial_input.toInt())*1000;
          Serial.println(hab/1000);
        }
      }
    }

    Serial1.println("ITI duration?");
    while(isi_a == -1){
      while(Serial1.available()){
        serial_input = Serial1.readStringUntil('\n');
        if(serial_input != -1){
          isi_a = (serial_input.toInt());
          isi_b = (serial_input.toInt()+1);
          Serial1.println(isi_a);          
        }
      }
    }

    Serial1.println("How many trials?");
    while(trial_set == -1){
      while(Serial1.available()){
        serial_input = Serial1.readStringUntil('\n');
        if(serial_input != -1){
          trial_set = serial_input.toInt();
          Serial1.println(trial_set);
        }
      }
    }
    
    Serial1.println("When does the US start after the CS onset?");
    while(us_start == -1){
      while(Serial1.available()){
        serial_input = Serial1.readStringUntil('\n');
        if(serial_input != -1){
          us_start = (serial_input.toInt())*1000;
          Serial1.println(us_start);
        }
      }
    }

    Serial1.println("US duration?");
    while(us_work == -1){
      while(Serial1.available()){
        serial_input = Serial1.readStringUntil('\n');
        if(serial_input != -1){
          us_work = (serial_input.toInt())*1000;
          Serial1.println(us_work);
        }
      }
    }

    Serial1.println("Which music do you want to play?");
    Serial1.println("a. Sin   b. whitenoise ");
    while(music == 0){
      while(Serial1.available()){
      char val = (char) Serial1.read();
        switch(val){
          case 'a':
            music = "sin1.wav";
            break;
          
          case 'b':
            music = "whitenoise.wav";
            break;

      
        }
          Serial1.println(music);
        }
      }
    
    //music = "sin1.wav";
    default_state = 0;
  }
  
  if(start_state == 0) {
    Serial1.println("Now what?");
    Serial1.println("a. start   b. reset   c. start_without_hab");
  }

  while(start_state != 1){
    while(Serial1.available())  //
    {
      char val = (char) Serial1.read();
      if(val != -1){
        switch(val){
          case 'a':
          Serial1.println("test start");
          test = 1;
          hstate = 0;
          starttime = millis();
          BT.write(letter_EXStart);
          Serial2.write(letter_EXStart);
          start_direction = millis() - ran1;
          break;
          
          case 'b':
          Serial1.println("option reset");
          person_state = 0;
          test_state = 0;
          speed1 = -1;
          isi_a = -1;
          hab = -1;
          us_start = -1;
          us_work = -1;
          trial_set = -1;
          music = 0;
          break;

          case 'c':
          Serial1.println("test start");
          test = 1;
          hstate = 1;
          starttime = millis();
          BT.write(letter_EXStart);
          Serial2.write(letter_EXStart);
          start_direction = millis() - ran1;
          break;

        
        }
        start_state = 1;
      }
    }
  }

  motordriver.setSpeed(speed1,MOTORB);
  motordriver.setSpeed(speed1,MOTORA);

  trial = 0;



 while(test == 1){
  time = millis() - starttime;
  prob = random(prob_a,prob_b);
  isi = random(isi_a, isi_b);
  isi = isi * 1000;
  trial = trial + 1;
  if(hstate == 0){
    Serial.print(time);
    Serial.println("Hab Start");
    Serial1.println("Hab Start");
    digitalWrite(LED_Pin, HIGH);
    delay(1000);
    digitalWrite(LED_Pin, LOW);
    trial = time_wait(hab-1000, trial_set, time_stamp);
    hstate = 1;
    Serial1.println("Hab End");
  }

  Serial.print(time);
  Serial1.println(trial);

  tmrpcm.disable();
  if(trial <= trial_set){
    time = millis() - starttime;
    Serial.print(time);
    Serial1.println("CS on");
    digitalWrite(LED_Pin, HIGH);
    
    tmrpcm.play(music);

    BT.write(letter_cson);
    Serial2.write(letter_cson);
    delay(1000);
    digitalWrite(41, LOW);
        
    delay(us_start-1000);

    select_rotate(start_direction, rotate_state, trial);
    
    delay(us_work);

    time = millis() - starttime;
    Serial.print(time);
    Serial1.println("CS US off");
    tmrpcm.disable();

    BT.write(letter_csoff);
    Serial2.write(letter_csoff);
    motordriver.stop();

    time = millis() - starttime;
    Serial.print(time);
    Serial1.println("ITI start");
    Serial1.println(isi/1000);
    trial = time_wait(isi, trial_set, time_stamp, trial);
    Serial1.println("ITI end\n");
    
  }

  if(trial > trial_set){
    Serial.print(time);
    Serial1.println("End Test\n");

    motordriver.stop();
    test = 0;
    BT.write(letter_EXEnd);
    Serial2.write(letter_EXEnd);
  }
 }

 test = 0;
 start_state = 0;

}




//
int time_wait(long wait_time, int trial_set, int time_stamp = 0, int trial = 0){
  long time_start = millis();

  long wait_time1 = wait_time;
  long time_check = 10000;
  int pause_state = 0;
  
  while(millis()-time_start < wait_time1){
    while(Serial1.available()){
      char val = (char) Serial1.read();
      if(val != -1){
        switch(val){
          case 'i': //
          Serial1.println("immediate exit");
          wait_time1 = 0;
          trial = trial_set + 1;
          break;

          case 't': //
          Serial1.println("Experiment is ongoing");
          Serial1.println("now trial");
          Serial1.println(trial);
          break;

          
          case 'p':
          Serial1.println("Pause");
          pause_state = 1;
          break;

          
          case 's':
          Serial1.println("start");
          pause_state = 0;
          time_start = millis();
          break;
          
          int remained_time = (wait_time1 - (millis()-time_start))/1000;
          Serial1.println(remained_time);
          break;

        }
        val = -1;
    }
   }
   if(pause_state == 1){
     time_start = millis();
   }
  if(time_stamp == 1 && millis()-time_start > time_check)
  {
    time_check = time_check + 10000; 
    Serial1.println((millis()-time_start)/1000);
  }
 }
 return trial;
}



void equip_check(int sd_test = 1 , int sound_test = 1, int motor_test = 1, int led_test = 1, int io_test = 1){  
  if(sd_test == 1){
    Serial1.println("SD_check");
    if (!SD.begin(SD_ChipSelectPin)) {  // see if the card is present and can be initialized:
      Serial1.println("No Sdcard");
    }
  else Serial1.println("Yes SDcard");
  }

  if(sound_test == 1){
    Serial1.println("Sound Check");
    tmrpcm.play("sin1.wav");
    break_test();
    tmrpcm.disable();
  }

  if(motor_test == 1){
    Serial1.println("Motor test");
    motordriver.goForward();
    break_test();
    motordriver.stop();
  }

  if(led_test == 1){
    Serial1.println("LED test");
    digitalWrite(LED_Pin, HIGH);
    break_test();
    digitalWrite(LED_Pin, LOW);  
  }

  if(io_test == 1){
    Serial1.println("IO test");
    BT.write(letter_cson);
    Serial2.write(letter_cson);
    BT.write(letter_EXStart);
    Serial2.write(letter_EXStart);
    break_test();
    BT.write(letter_csoff);
    Serial2.write(letter_csoff);
    BT.write(letter_EXEnd);
    Serial2.write(letter_EXEnd);    
  }
}


void break_test(){
  int test_state = 1;
  while(test_state == 1){
    while(Serial1.available()){
        Serial1.println("Please press 'n'");
        char val = (char) Serial1.read();
        if(val != -1){
          switch(val){
            case 'n':
            test_state = 0;
            Serial1.println("Check end");
            break;
        }
      }
    }
  }
}

void select_rotate(long start_direction, int rotate_state, int trial){
  Serial1.println(start_direction);
  int rotate_direction = start_direction % 2;
  if(rotate_state == 0){
    motor_move(0);
  }
  else if(rotate_state == 1){ //one-way rotate
    motor_move(rotate_direction);
  }
  else if(rotate_state == 2){//
    rotate_direction = (rotate_direction + trial) % 2;
    motor_move(rotate_direction);
  }
  else if(rotate_state == 3){
    randomSeed(millis());
    rotate_direction = (rotate_direction + random(2)*trial) % 2;
    motor_move(rotate_direction);
  }
}

void motor_move(int rotate_direction){
  if(rotate_direction == 0){
    motordriver.goForward();
    Serial1.println("Go Forward");        
  }
  else{
    motordriver.goBackward();
    Serial1.println("Go Backward");
  }
}
 /* 
 * SD card ::  cs - 53 / DI - 51 DO - 50 / CLK - 52 / GND / 5V
 * Bluetooth :: RX -18 / TX - 19 / GND / 3.3V
 * Sound Pin :: 6(Mono),7(Stereo)
 * Motordriver :: 8~13(Please refer to MotorDriver.h)
 * music_encoding : unsigned 8bit pcm/ 8~32khz / mono
 */
