#include "/Users/lingdongh/Documents/Arduino/ICHING/OLEDCTRL.TXT"

#define MTR_TL 18
#define MTR_TM 16
#define MTR_TR 10

#define MTR_BL 19
#define MTR_BM 17
#define MTR_BR 11

#define MTR_ZZ 22

#define N_MTR 7
const int MTRS[N_MTR] = {MTR_TL,MTR_TM,MTR_TR,MTR_BL,MTR_BM,MTR_BR,MTR_ZZ};

int MTR_ZEROS[32] = {0};
int MTR_DIRS [32] = {0};

#define INIT_MTR_ZEROS \
MTR_ZEROS[MTR_TL] = 500;\
MTR_ZEROS[MTR_TM] = 1350;\
MTR_ZEROS[MTR_TR] = 1250;\
MTR_ZEROS[MTR_BL] = 580;\
MTR_ZEROS[MTR_BM] = 1450;\
MTR_ZEROS[MTR_BR] = 1400;\
MTR_ZEROS[MTR_ZZ] = 800;\

#define INIT_MTR_DIRS \
MTR_DIRS[MTR_TL] = 1;\
MTR_DIRS[MTR_TM] = -1;\
MTR_DIRS[MTR_TR] = 1;\
MTR_DIRS[MTR_BL] = 1;\
MTR_DIRS[MTR_BM] = -1;\
MTR_DIRS[MTR_BR] = 1;\

#define LED_T 4
#define SEN_T 2

#define LED_B 5
#define SEN_B 3

#define IND_0 27
#define IND_1 28

#define SEN_THRESH 80



#define SER Serial2


bool oled_did_init = false;

#define dbgp(x) {SER.println((x)); Serial.println((x)); if (oled_did_init) {OLED3x7string_direct(0,4,"                                ");OLED3x7string_direct(0,4,(x));} }
#define nts(x) (char*)String(x).c_str()

void pwm_quick(int pin, int a){
  int b = 20000-a;
  for (int i = 0; i < 2; i++){
    digitalWrite(pin,HIGH);
    delayMicroseconds(a);
    digitalWrite(pin,LOW);
    delayMicroseconds(b);
  }
}

void pwm_full(int pin, int a){
  int b = 20000-a;
  for (int i = 0; i < 15; i++){
    digitalWrite(pin,HIGH);
    delayMicroseconds(a);
    digitalWrite(pin,LOW);
    delayMicroseconds(b);
  }
}

void crawl(int pin, int a, int b){
//  dbgp("crawling...");
//  dbgp(nts(a));
//  dbgp(nts(b));
  if (a < b){
    for (int i = a; i < b; i+=50){
//      dbgp(nts(i));
      pwm_full(pin,i);
      delay(1);
    }
  }else{
    for (int i = a; i > b; i-=50){
//      dbgp(nts(i));
      pwm_full(pin,i);
      delay(1);
    }
  }
  pwm_full(pin,b);
//  dbgp("ok!");
}


void quick_door(int mtr){
  int zero = MTR_ZEROS[mtr];
  int dir = MTR_DIRS[mtr];
  pwm_quick(mtr,zero);
  pwm_quick(mtr,zero+200*dir);
  delay(60);
  pwm_quick(mtr,zero);
}


void slow_door(int mtr){
  int zero = MTR_ZEROS[mtr];
  int dir = MTR_DIRS[mtr];
  pwm_full(mtr,zero);
  pwm_full(mtr,zero+300*dir);
  delay(1000);
  pwm_full(mtr,zero);
}


void gate_open(int mtr){
  int zero = MTR_ZEROS[mtr];
  int dir = MTR_DIRS[mtr];
//  pwm_full(mtr,zero);
  pwm_full(mtr,zero+200*dir);
}

void gate_close(int mtr){
  int zero = MTR_ZEROS[mtr];
  int dir = MTR_DIRS[mtr];
//  pwm_full(mtr,zero+200*dir);
  pwm_full(mtr,zero);
}


void hammer_open(int mtr){
  int zero = MTR_ZEROS[mtr];
  int dir = MTR_DIRS[mtr];
  pwm_full(mtr,zero+100*dir);
}

void hammer_close(int mtr){
  int zero = MTR_ZEROS[mtr];
  int dir = MTR_DIRS[mtr];
  pwm_full(mtr,zero);
}


float sense_light(int sen_pin, int led_pin, bool led_on, float blend){
  int n = 200;
  float avg = 0;
  float mxm = 0;
  digitalWrite(led_pin,led_on?HIGH:LOW);
  for (int j = 0; j < n; j++){
    int a = analogRead(sen_pin);
    avg += a;
    mxm = max(a,mxm);
  }
  digitalWrite(led_pin,LOW);
  avg /= (float)n;
  return avg*(1-blend)+mxm*blend;
}


int sensor_iter(int door_pin, int sen_pin, int led_pin, int thresh){
  float v0, v1;
  int zero = MTR_ZEROS[MTR_ZZ];

  crawl(MTR_ZZ, zero,zero+200);
  crawl(MTR_ZZ, zero+200,zero-300);
  pwm_full(MTR_ZZ, zero-200);
  pwm_full(MTR_ZZ, zero-300);
  pwm_full(MTR_ZZ, zero-200);
  pwm_full(MTR_ZZ, zero-300);
  
  
  v0 = sense_light(sen_pin,led_pin,true,0);
  delay(1000);
  quick_door(door_pin);
//  delay(50);
  v1 = sense_light(sen_pin,led_pin,true,0.8);
  
  dbgp(nts(v0));
  dbgp(nts(v1));
  
  crawl(MTR_ZZ, zero-300,zero);

  if (v1>v0+thresh){
    dbgp("detect!");
    return 1;
  }else{
    dbgp("no detect!");
    return 0;
  }
}


int splitter_iter(){
  int zero = MTR_ZEROS[MTR_ZZ];
  
  gate_open(MTR_TM);
  crawl(MTR_ZZ, zero,zero+100);
  crawl(MTR_ZZ, zero+100,zero-150);
  crawl(MTR_ZZ, zero-150, zero+100);
  crawl(MTR_ZZ, zero+100, zero);
  gate_close(MTR_TM);
}

void auto_clean(){
  int zero = MTR_ZEROS[MTR_ZZ];
  
  gate_close(MTR_TM);
  gate_close(MTR_BM);
  
  hammer_close(MTR_TR);
  hammer_open(MTR_TL);
  hammer_open(MTR_BL);
  
  crawl(MTR_ZZ, zero, zero-300);

  for (int i = 0; i < 4; i++){
    crawl(MTR_ZZ, zero-300, zero+100);
    crawl(MTR_ZZ, zero+100, zero-300);
    delay(1000);
  }
  hammer_close(MTR_TL);
  for (int i = 0; i < 2; i++){
    crawl(MTR_ZZ, zero-300, zero+100);
    crawl(MTR_ZZ, zero+100, zero-300);
    delay(1000);
  }
  hammer_close(MTR_BL);
  
  crawl(MTR_ZZ,zero-300,zero+1000);
  hammer_open(MTR_BR);
  hammer_open(MTR_TR);
  
  for (int i = 0; i < 5; i++){
    crawl(MTR_ZZ, zero+1000, zero+1400);
    crawl(MTR_ZZ, zero+1400, zero+1000);
    delay(1000);
  }
  hammer_close(MTR_BR);
  for (int i = 0; i < 2; i++){
    crawl(MTR_ZZ, zero+1000, zero+1400);
    crawl(MTR_ZZ, zero+1400, zero+1000);
    delay(1000);
  }
  hammer_close(MTR_TR);
  crawl(MTR_ZZ, zero+1000, zero);
  crawl(MTR_ZZ, zero, zero+1000);
                   
  hammer_open(MTR_BR);
  hammer_open(MTR_TR);
  for (int i = 0; i < 2; i++){
    crawl(MTR_ZZ, zero+1000, zero+1400);
    crawl(MTR_ZZ, zero+1400, zero+1000);
    delay(1000);
  }
  hammer_close(MTR_BR);
  hammer_close(MTR_TR);
  crawl(MTR_ZZ, zero+1000, zero);
}


int xx_iter(int total){
  int zero = MTR_ZEROS[MTR_ZZ];
  splitter_iter();

  int rest = 0;

  int cnt0 = 0;
  int miss = 0;
  for (int i = 0; i < total; i++){
    int a = sensor_iter(MTR_TL,SEN_T,LED_T,SEN_THRESH);
    cnt0 += a;
    if (!a){
      miss++;
    }else{
      miss = 0;
    }
    if (miss >= 4){
      break;
    }
  }
  rest = total-cnt0;
  int cnt1 = (cnt0+3)%4+1;
  int cnt2 = (rest+2)%4+2;
  
  int acm = (cnt0 - cnt1) + (rest - cnt2);
  
  for (int i = 0; i < total; i++){
    int a = sensor_iter(MTR_BL,SEN_B,LED_B,SEN_THRESH);
    cnt1 -= a;
    if (cnt1 == 0){
      break;
    }
  }
  
  gate_open(MTR_BM);
  crawl(MTR_ZZ,zero,zero+500);
  for (int i = 0; i < 3; i++){
    crawl(MTR_ZZ,zero+500,zero+600);
    crawl(MTR_ZZ,zero+600,zero+500);
  }
  gate_close(MTR_BM);
  
  crawl(MTR_ZZ,zero+500,zero);

  gate_open(MTR_TM);

  crawl(MTR_ZZ,zero,zero-300);
  crawl(MTR_ZZ,zero-300,zero+100);
  crawl(MTR_ZZ,zero+100,zero-400);
  delay(1000);
  gate_close(MTR_TM);
  crawl(MTR_ZZ,zero-400,zero);
  
  crawl(MTR_ZZ,zero,zero+1000);
  hammer_open(MTR_TR);
  for (int i = 0; i < 3; i++){
    crawl(MTR_ZZ, zero+1000, zero+1400);
    crawl(MTR_ZZ, zero+1400, zero+1000);
    delay(800);
    for (int j = 0; j < 3; j++){
      pwm_full(MTR_ZZ, zero+1000);
      pwm_full(MTR_ZZ, zero+1100);
    }
    delay(800);
    crawl(MTR_ZZ, zero+1100, zero+1000);
  }
  hammer_close(MTR_TR);
  crawl(MTR_ZZ, zero+1000, zero);


  /////////////////////////////////////////////////////////////////
  

  hammer_open(MTR_TL);
  crawl(MTR_ZZ, zero, zero-300);

  for (int i = 0; i < 3; i++){
    crawl(MTR_ZZ, zero-300, zero+100);
    crawl(MTR_ZZ, zero+100, zero-300);
    delay(800);
  }

  crawl(MTR_ZZ, zero-300, zero);
  hammer_close(MTR_TL);

  for (int i = 0; i < total; i++){
    int a = sensor_iter(MTR_BL,SEN_B,LED_B,SEN_THRESH);
    cnt2 -= a;
    if (cnt2 == 0){
      break;
    }
  }

  gate_open(MTR_BM);
  crawl(MTR_ZZ,zero,zero+500);
  for (int i = 0; i < 3; i++){
    crawl(MTR_ZZ,zero+500,zero+600);
    crawl(MTR_ZZ,zero+600,zero+500);
  }
  gate_close(MTR_BM);

  crawl(MTR_ZZ,zero+500,zero);


  crawl(MTR_ZZ,zero,zero+1000);
  hammer_open(MTR_TR);
  for (int i = 0; i < 3; i++){
    crawl(MTR_ZZ, zero+1000, zero+1400);
    crawl(MTR_ZZ, zero+1400, zero+1000);
    delay(800);
    for (int j = 0; j < 3; j++){
      pwm_full(MTR_ZZ, zero+1000);
      pwm_full(MTR_ZZ, zero+1100);
    }
    delay(800);
    crawl(MTR_ZZ, zero+1100, zero+1000);
  }
  hammer_close(MTR_TR);
  crawl(MTR_ZZ, zero+1000, zero);

  return acm;
}


void ind_begin(){
   SER.println("$g");
   digitalWrite(IND_0,LOW);
}

void ind_end(){
   SER.println("$G");
   digitalWrite(IND_0,HIGH);
}

void read_serial(){
   while (SER.available() > 0) {
    char b = SER.read();
    if (b == '0'){
      dbgp("$gG");
      dbgp("Master ready.");
      
    }else if (b == '1'){
      dbgp("Initializing display...");
      ind_begin();
      OLEDsetup();
      oled_did_init = true;
      dbgp("Display ready.");
      ind_end();
      
    }else if (b == '2'){
      dbgp("Zero base in 3s. Remove box!");
      delay(3000);
      ind_begin();
      pwm_full(MTR_ZZ,MTR_ZEROS[MTR_ZZ]);
      ind_end();
      dbgp("Zeroed. Place box!");
      
    }else if (b == '3'){
      dbgp("Zeroing doors...");
      for (int i = 0; i < N_MTR-1; i++){
        int mtr = MTRS[i];
        dbgp(nts(i));
        delay(500);
        ind_begin();
        pwm_full(mtr,MTR_ZEROS[mtr]);
        ind_end();
      }
      dbgp("Zeroed doors.");
    }else if (b == '4'){
      
      dbgp("Testing doors...");

      dbgp("Top left");
      ind_begin();
      quick_door(MTR_TL);
      ind_end();
      delay(500);

      dbgp("Top middle");
      ind_begin();
      gate_open(MTR_TM);
      gate_close(MTR_TM);
      ind_end();
      delay(500);
      
      dbgp("Top right");
      ind_begin();
      hammer_open(MTR_TR);
      delay(1000);
      hammer_close(MTR_TR);
      ind_end();
      delay(500);

      dbgp("Bottom left");
      ind_begin();
      quick_door(MTR_BL);
      ind_end();
      delay(500);

      dbgp("Bottom middle");
      ind_begin();
      gate_open(MTR_BM);
      gate_close(MTR_BM);
      ind_end();
      delay(500);
      
      dbgp("Bottom right");
      ind_begin();
      hammer_open(MTR_BR);
      delay(1000);
      hammer_close(MTR_BR);
      ind_end();
      delay(500);

      dbgp("Door test done.");

    }else if (b == '5'){
      float v0, v1;
      
      dbgp("Testing LED/sensor");

      ind_begin();
      v0 = sense_light(SEN_T,LED_T,true,0);
      ind_end();
      dbgp(nts(v0));

      delay(1000);

      ind_begin();
      v1 = sense_light(SEN_T,LED_T,false,0);
      ind_end();
      dbgp(nts(v1));
      
      delay(1000);

      ind_begin();
      v0 = sense_light(SEN_B,LED_B,true,0);
      ind_end();
      dbgp(nts(v0));

      delay(1000);

      ind_begin();
      v1 = sense_light(SEN_B,LED_B,false,0);
      ind_end();
      dbgp(nts(v1));

    }else if (b == '6'){
//      int zero = MTR_ZEROS[MTR_ZZ];
//
//      ind_begin();
//      crawl(MTR_ZZ, zero,zero-300);
//      crawl(MTR_ZZ, zero-300,zero+1500);
//      crawl(MTR_ZZ, zero+1500,zero);
//      ind_end();
//      pwm_full(MTR_ZZ, zero-300);
      sensor_iter(MTR_TL,SEN_T,LED_T,SEN_THRESH);
    }else if (b == '7'){
    
      int a = xx_iter(49);
      
      dbgp(nts(a));

    }else if (b == '8'){
//      splitter_iter();


      int result[6] = {0};
      for (int i = 0; i < 6; i++){
        int total = 49;
        while (total != 6*4 && total != 7*4 && total != 8*4 && total != 9*4){
          int a = xx_iter(total);
          total=a;
        }
        result[i] = total/4;
        auto_clean();
      }
      dbgp( (char*)(String(result[0])+String(result[1])+String(result[2])+String(result[3])+String(result[4])+String(result[5])).c_str() );
      
      OLEDresult(result[0],result[1],result[2],result[3],result[4],result[5]);
    }else if (b == '9'){
      auto_clean();
      
    }else if (b == 'a'){
      sensor_iter(MTR_BL,SEN_B,LED_B,SEN_THRESH);
    }else if (b == 'b'){
      OLEDresult(8,8,6,9,8,8);
    }
  }
}

#define DRAW_YIN {for (int i = 0; i < 20; i++) OLEDdata(60); for (int i = 0; i < 8; i++) OLEDdata(0); for (int i = 0; i < 20; i++) OLEDdata(60); }
#define DRAW_YNG {for (int i = 0; i < 48; i++) OLEDdata(60);}

void OLEDresult(int a, int b, int c, int d, int e, int f){
  int x0 = a % 2;
  int x1 = b % 2;
  int x2 = c % 2;
  int x3 = d % 2;
  int x4 = e % 2;
  int x5 = f % 2;

  int y0 = ( a % 3 ) ? x0 : (!x0);
  int y1 = ( b % 3 ) ? x1 : (!x1);
  int y2 = ( c % 3 ) ? x2 : (!x2);
  int y3 = ( d % 3 ) ? x3 : (!x3);
  int y4 = ( e % 3 ) ? x4 : (!x4);
  int y5 = ( f % 3 ) ? x5 : (!x5);

  OLEDclear();
  OLEDclear();
  OLEDgoto(1,8); if (x0) DRAW_YNG else DRAW_YIN;
  OLEDgoto(2,8); if (x1) DRAW_YNG else DRAW_YIN;
  OLEDgoto(3,8); if (x2) DRAW_YNG else DRAW_YIN;
  OLEDgoto(4,8); if (x3) DRAW_YNG else DRAW_YIN;
  OLEDgoto(5,8); if (x4) DRAW_YNG else DRAW_YIN;
  OLEDgoto(6,8); if (x5) DRAW_YNG else DRAW_YIN;
  OLEDgoto(1,72); if (y0) DRAW_YNG else DRAW_YIN;
  OLEDgoto(2,72); if (y1) DRAW_YNG else DRAW_YIN;
  OLEDgoto(3,72); if (y2) DRAW_YNG else DRAW_YIN;
  OLEDgoto(4,72); if (y3) DRAW_YNG else DRAW_YIN;  
  OLEDgoto(5,72); if (y4) DRAW_YNG else DRAW_YIN;  
  OLEDgoto(6,72); if (y5) DRAW_YNG else DRAW_YIN;  
  
}

void setup() {
  
  digitalWrite(IND_0,HIGH);
  
  for (int i = 0; i < N_MTR; i++) pinMode(MTRS[i], OUTPUT);
  INIT_MTR_ZEROS;
  INIT_MTR_DIRS;

  pinMode(SEN_T,INPUT);
  pinMode(SEN_B,INPUT);
  
  pinMode(LED_T,OUTPUT);
  pinMode(LED_B,OUTPUT);
  
  pinMode(IND_0,OUTPUT);
  pinMode(IND_1,OUTPUT);

  SER.begin(9600);
  SER.println("$gG");

  Serial.begin(9600);
  
  digitalWrite(IND_1,HIGH);



}

void loop() {
  read_serial();
}