#include <LiquidCrystal_I2C.h>
#include <Wire.h>
LiquidCrystal_I2C lcd(0x27, 20, 4); //設定LCD位置0x27,設定LCD大小為20*4
//**************************************************************************************//
const byte encoder1_A=22; //1byte=8bit省空間
const byte encoder1_B=23;
const byte encoder2_A=24;
const byte encoder2_B=25;
const byte encoder3_A=26;
const byte encoder3_B=27;
const byte encoder4_A=28;
const byte encoder4_B=29;
const byte encoder5_A=30;
const byte encoder5_B=31;
const byte encoder6_A=32;
const byte encoder6_B=33;
const byte encoder7_A=34;
const byte encoder7_B=35;
//**************************************************************************************//
#define detection_time 5   //auto mode 5s偵測一次
#define cv_position_range 12 //容許誤差 (unit:pulse)
#define gear_rate 1//2.3561925 //unit mm/pulse
//**************************************************************************************//
int mode=4;//預設開機為mode4
int mode2_case,mode4_case;
int mode_t_1;
int comparator_status=0;//
int measure_force_start=0;

volatile long counter[8]={0,0,0,0,0,0,0,0};  //7組convey，挖謀尬藝從0開始
//long counter[8]={0,0,0,0,0,0,0,0};  //7組convey，挖謀尬藝從0開始

int x=0,y=0;//data buffer 
int databuffer[100];//data buffer 
/*-------------------------------------------------------------------------------------------*/
int times_ms = 0,times_s=0;//全域用
//**************************************************************************************//
void setup() 
{  
 Serial.begin(9600);

  pinMode(encoder1_A, INPUT_PULLUP);//ENCODE1_A
  pinMode(encoder1_B, INPUT_PULLUP);//ENCODE1_B
  pinMode(encoder2_A, INPUT_PULLUP);//ENCODE2_A
  pinMode(encoder2_B, INPUT_PULLUP);//ENCODE2_B
  pinMode(encoder3_A, INPUT_PULLUP);//ENCODE3_A
  pinMode(encoder3_B, INPUT_PULLUP);//ENCODE3_B
  pinMode(encoder4_A, INPUT_PULLUP);//ENCODE4_A
  pinMode(encoder4_B, INPUT_PULLUP);//ENCODE4_B
  pinMode(encoder5_A, INPUT_PULLUP);//ENCODE5_A
  pinMode(encoder5_B, INPUT_PULLUP);//ENCODE5_B
  pinMode(encoder6_A, INPUT_PULLUP);//ENCODE6_A
  pinMode(encoder6_B, INPUT_PULLUP);//ENCODE6_B
  pinMode(encoder7_A, INPUT_PULLUP);//ENCODE7_A
  pinMode(encoder7_B, INPUT_PULLUP);//ENCODE7_B

  pinMode(8, INPUT_PULLUP);//ENCODE1_Z
  pinMode(11, INPUT_PULLUP);//counter reset_button
  pinMode(12, INPUT_PULLUP);//Mode1_button
  pinMode(13, INPUT_PULLUP);//Mode2_button


  lcd.init(); //初始化LCD
  lcd.backlight(); //開啟背光
  lcd.setCursor(1,0);
  lcd.print("Amount of Movement");
  lcd.setCursor(0,1);
  lcd.print("Detect for Multi C/V"); 
  lcd.setCursor(4,2);
  lcd.print("INX LCD6 BEOL");  
  lcd.setCursor(4,3);
  lcd.print(" T.Y. 61802");
  delay(1500);
  //lcd.init();
  //lcd.noBacklight();

  /*-------------------------------------  <時間中斷設定> -------------------------------------*/
  cli();  // 禁止中斷
  TCCR1A = 0;
  TCCR1B = 0;
  TCCR1B |= (1 << WGM12); // CTC mode; Clear Timer on Compare
  TCCR1B |= (1 << CS10) | (1 << CS11); // Prescaler == 64 //(1<<CS10) | (1<<CS12);  //Prescaler == 1024
  OCR1A = 250;//24999;  //250=1k;125=2Khz// 1/(16MHZ/Prescaler)/(中斷間格時間)
  TCNT1 = 0; // counter 歸零
  TIMSK1 |= (1 << OCIE1A);  // enable CTC for TIMER1_COMPA_vect--TIMER1 CH2

  //OCR1B = 249990/2;
  //TIMSK1 |= (1 << OCIE1B);  // enable CTC for TIMER1_COMPB_vect--TIMER1 CH2

  sei();  // 允許中斷
}

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

void loop() 
{   
  unsigned int button_comb=!digitalRead(13)<<2 | !digitalRead(12)<<1 |!digitalRead(11);
  switch (button_comb)
  {
    case 1:  
    mode_t_1=mode; //initial counter前先記錄切換前是哪個mode
    mode=1;
    break;
    
    case 2:
    mode=2;
    break;

    case 4:
    mode=4;
    break; 
  }

  counter[1]=encoder_counter(digitalRead(encoder1_A),digitalRead(encoder1_B),1);
  counter[2]=encoder_counter(digitalRead(encoder2_A),digitalRead(encoder2_B),2);
  counter[3]=encoder_counter(digitalRead(encoder3_A),digitalRead(encoder3_B),3);
  counter[4]=encoder_counter(digitalRead(encoder4_A),digitalRead(encoder4_B),4);
  counter[5]=encoder_counter(digitalRead(encoder5_A),digitalRead(encoder5_B),5);
  counter[6]=encoder_counter(digitalRead(encoder6_A),digitalRead(encoder6_B),6);
  counter[7]=encoder_counter(digitalRead(encoder7_A),digitalRead(encoder7_B),7);
  
  counter_initial_function(differential(digitalRead(13), 13),0);
  counter_initial_function(differential(digitalRead(12), 12),0);
  counter_initial_function(differential(digitalRead(11), 11),0);
//------------------------------------------------------------------------------------------------//
  if(change(mode,1))
  {
    mode2_case=1;
    mode4_case=1;
  }

  switch(mode)
  {   
   case 1:     //initial mode
     times_ms=0;
     times_s=0;
     measure_force_start=0;
     lcd_message (0,0);
     mode=mode_t_1; //
   break;
     
   case 2:      //量測模式 
    switch(mode2_case)
    {
      case 1:
             lcd_message (1,0);//step1
             mode2_case=2;
      break;

      case 2:  
             if(counter[1]-counter[2]-counter[3]-counter[4]-counter[5]-counter[6]-counter[7]==0 && measure_force_start==0 )
             {
              TIMSK1 = 0;
              times_s =0;
              times_ms=0; 
             }
             else
             {
               if(times_s==10)// && TIMSK1!=0)
               {
                 TIMSK1 = 0;//停止TIME1中斷
                 measure_force_start==0;
                 lcd_message (6,0);
                  
               }
               else if (times_s < 10)
               {
                TIMSK1 |= (1 << OCIE1A); //啟用TIME1 CH_A中斷
                measure_force_start=1;
                lcd_message (3, 1); 
               }
             }
       break;
     }
     break;
    
   case 4:      //全時間控模式

    switch(mode4_case)
    {
      case 1:
            lcd_message (2,0); 
            mode4_case=2;
      break;

      case 2:
            if(times_s==detection_time )//&& TIMSK1!=0)
            {
              comparator_status=comparator();// comparator靠判定counter歸零時也歸零 不須另外強制歸零
              if(comparator_status==0)
              {
               TIMSK1 = 0;//停止TIME1中斷
               times_s =0;
               times_ms=0;
               lcd_message (4,1);
               counter_initial_function(0,1);
              }
              else 
              {
                //TIMSK1 = 0;//停止TIME1中斷
                lcd_message (5,0);
                mode4_case=3;
              }   

            }
            else if (times_s < detection_time)
             {
               TIMSK1 |= (1 << OCIE1A); //啟用TIME1 CH_A中斷
             }
       break;
      
      case 3:
            if(comparator_status==0)
            {
             mode4_case=2;
            }
            TIMSK1 |= (1 << OCIE1A);
            lcd_message (3,1);
       break;      

    }

    break;
  }


  
}
/*----------------------------------------------------------------------------------------------------------------------------//
//***************************************************************************************/
/******************************  <時間中斷副程式>  ****************************************/
/***************************************************************************************/
ISR(TIMER1_COMPA_vect)//f=1Khz
{
  ++times_ms; 
  if(times_ms==1000)
  {
    times_s++;
    times_ms=0;    
  }
}
//----------------------------------------------------------------------------------------------------------------------------//
//----------------------------------------------------------------------------------------------------------------------------//
/**************************************************************************************/
/*******************************  <Comparator>  ***************************************/
/*************************<comparator: 1: NG ; 0: OK > ********************************/
/**************************************************************************************/
int comparator()
{
  int comparator=0;

  for(int x=1;x<7;x++)
  {
    for(int y=x+1;y<8;y++)
     {

       if(abs(counter[x]-counter[y]) < cv_position_range)
       {
        comparator==0;
       }
       else
       {
         comparator=1;
         x=8;
         y=9;
       }
          
     }
  }

  return comparator;
}

//----------------------------------------------------------------------------------------------------------------------------//
/**************************************************************************************/
/**************************  <Enocder Counder>  ***************************************/
/**************************************************************************************/

long encoder_counter (int pulse_a , int pulse_b,int cv_no)
{
  static int pulse_a_t_1[8];//user for Enocder Counter function //7組convey
  static int pulse_b_t_1[8];//user for Enocder Counter  function //7組convey
  unsigned int comb=pulse_a_t_1[cv_no]<<3 | pulse_b_t_1[cv_no]<<2 | pulse_a<<1 | pulse_b;

  switch(comb)
  {
   case 1:
   counter[cv_no]=counter[cv_no]+1;
   break;
   case 7:
   counter[cv_no]=counter[cv_no]+1;  
   break;
   case 14: 
   counter[cv_no]=counter[cv_no]+1;    
   break;
   case 8: 
   counter[cv_no]=counter[cv_no]+1;  
   break;

   case 2:
   counter[cv_no]=counter[cv_no]-1;
   break;
   case 11:
   counter[cv_no]=counter[cv_no]-1;  
   break;
   case 13: 
   counter[cv_no]=counter[cv_no]-1;    
   break;
   case 4: 
   counter[cv_no]=counter[cv_no]-1;    
   break;
  }
  pulse_a_t_1[cv_no]=pulse_a;
  pulse_b_t_1[cv_no]=pulse_b;
  return counter[cv_no];
}
//----------------------------------------------------------------------------------------------------------------------------//
/**************************************************************************************/
/*****************************  <change=訊號微分函式>  ***************************************/
/**************************************************************************************/
int change (int input_signal , int no)
{
  int change;
  static int change_t_1_signal[99];//開99組chaung訊號
  
  if (input_signal != change_t_1_signal[no])
  {
    change = 1;
  }
  else
  {
    change = 0;
  }
  change_t_1_signal[no] = input_signal;
  return change;
}
/**************************************************************************************/
/*****************************  <訊號微分函式>  ***************************************/
/**************************************************************************************/
int differential (int input_signal , int pin_no)
{
  int differential;
  static int differential_t_1_signal[14];//user for differential function //14組 input signal
  
  if (input_signal != differential_t_1_signal[pin_no])
  {
    differential = 1;
  }
  else
  {
    differential = 0;
  }
  differential_t_1_signal[pin_no] = input_signal;
  return differential;
}
//----------------------------------------------------------------------------------------------------------------------------//
/*****************************  <上緣觸發函式>  ***************************************/
/**************************************************************************************/
int rising (int input_signal , int pin_no)
{
  int rising;
  static int rising_t_1_signal[14];//user for loop function //14組 input signal
  
  if (input_signal - rising_t_1_signal[pin_no] > 0)
  {
    rising = 1;
  }
  else
  {
    rising = 0;
  }
  rising_t_1_signal[pin_no] = input_signal;
  return rising;
}
//--------------------------------------------------------------------------------------------------------------------------//
/*****************************  <下緣觸發函式>  ***************************************/
/**************************************************************************************/
int falling (int input_signal , int pin_no)
{
  int falling;
  static int falling_t_1_signal[14];//user for loop function //14組 input signal
  
  if (input_signal - falling_t_1_signal[pin_no] < 0)
  {
    falling = 1;
  }
  else
  {
    falling = 0;
  }
  falling_t_1_signal[pin_no] = input_signal;
  return falling;
}
//--------------------------------------------------------------------------------------------------------------------------//
/***************************  <counter initial函式>  **********************************/
/**************************************************************************************/
void counter_initial_function (int initial_trigger_signal,int force_trigger) 
{
  static int counter_initial_flag = 1;
  
  if(initial_trigger_signal==1||force_trigger==1)
  {
   counter_initial_flag=1;
  }
  while(counter_initial_flag)
  {   
    //comparator_status=0;
    counter[1]=0;
    counter[2]=0; 
    counter[3]=0; 
    counter[4]=0;
    counter[5]=0;
    counter[6]=0; 
    counter[7]=0;   
    counter_initial_flag=0;
  }

}
/***************************  <LCD Message函式>  **************************************/
/***<lcd_message:罐頭訊息 by case
     lcd_display_mode:0->在mode切換後reflash*1 
                      1->在mode切換後reflash*infinity>***/
/**************************************************************************************/
void lcd_message (int lcd_message,int lcd_display_mode) 
{
  static int lcd_display_mode_flag;
  static int lcd_message_t_1;
  

  if(lcd_message_t_1!=lcd_message &&lcd_display_mode==0)
  {
   lcd_display_mode_flag=1;
  }
  else if (lcd_message_t_1==lcd_message &&lcd_display_mode==0)
  {
   lcd_display_mode_flag=0;
  }    
  else
  {
   lcd_display_mode_flag=1;
  }
  
  lcd_message_t_1=lcd_message;
    

  if(lcd_display_mode_flag==1)
  {
   switch(lcd_message)
   {   
    case 0:
     lcd.init(); //初始化LCD
     lcd.backlight(); //開啟背光
     lcd.setCursor(0, 0);lcd.print("********************");      
     lcd.setCursor(0, 1);lcd.print("***<<<Counter>>>>***"); 
     lcd.setCursor(0, 2);lcd.print("***<<<Initial>>>>***"); //顯示 
     lcd.setCursor(0, 3);lcd.print("********************");    
    break;
     
    case 1:
     lcd.init(); //初始化LCD
     lcd.backlight(); //開啟背光
     lcd.setCursor(3, 0);
     lcd.print("Manual Mode!!!"); //顯示
     lcd.setCursor(1, 1);
     lcd.print("Start Measurement");
     lcd.setCursor(8, 2);
     lcd.print("10sec");
     lcd.setCursor(3, 3);
     lcd.print("After C/V Move");
     //lcd.setCursor(3, 3);
     //lcd.setCursor(0, 0); lcd.print(++x);      
    break;

    case 2:
     lcd.init(); //初始化LCD
     lcd.backlight(); //開啟背光
     //lcd.backlight(); //開啟背光
     //lcd.setCursor(0, 3); lcd.print(++y); 
     lcd.setCursor(3, 1);
     lcd.print("Auto Mode!!!"); //顯示
    break;
    
    case 3:
     if(times_ms == 0)
     {
       lcd.noBacklight();
     }
     else if (times_ms==499 )
     {
       lcd.backlight();
     }
    break; 

    case 4:
     lcd.init(); //初始化LCD
     lcd.backlight(); //開啟背光 
     lcd.setCursor(0, 0);lcd.print("1:"); 
     lcd.setCursor(2, 0);lcd.print(counter[1]*gear_rate);
     lcd.setCursor(0, 1);lcd.print("2:"); 
     lcd.setCursor(2, 1);lcd.print(counter[2]*gear_rate);  
     lcd.setCursor(0, 2);lcd.print("3:"); 
     lcd.setCursor(2, 2);lcd.print(counter[3]*gear_rate);  
     lcd.setCursor(0, 3);lcd.print("4:"); 
     lcd.setCursor(2, 3);lcd.print(counter[4]*gear_rate); 

     lcd.setCursor(11, 0);lcd.print("5:"); 
     lcd.setCursor(13, 0);lcd.print(counter[5]*gear_rate);
     lcd.setCursor(11, 1);lcd.print("6:"); 
     lcd.setCursor(13, 1);lcd.print(counter[6]*gear_rate);  
     lcd.setCursor(11, 2);lcd.print("7:"); 
     lcd.setCursor(13, 2);lcd.print(counter[7]*gear_rate); 
     lcd.setCursor(13, 3);lcd.print("Monitor"); 
 
    break; 

    case 5:
     lcd.init(); //初始化LCD
     lcd.backlight(); //開啟背光 
     lcd.setCursor(0, 0);lcd.print("1:"); 
     lcd.setCursor(2, 0);lcd.print(counter[1]*gear_rate);
     lcd.setCursor(0, 1);lcd.print("2:"); 
     lcd.setCursor(2, 1);lcd.print(counter[2]*gear_rate);  
     lcd.setCursor(0, 2);lcd.print("3:"); 
     lcd.setCursor(2, 2);lcd.print(counter[3]*gear_rate);  
     lcd.setCursor(0, 3);lcd.print("4:"); 
     lcd.setCursor(2, 3);lcd.print(counter[4]*gear_rate); 

     lcd.setCursor(11, 0);lcd.print("5:"); 
     lcd.setCursor(13, 0);lcd.print(counter[5]*gear_rate);
     lcd.setCursor(11, 1);lcd.print("6:"); 
     lcd.setCursor(13, 1);lcd.print(counter[6]*gear_rate);  
     lcd.setCursor(11, 2);lcd.print("7:"); 
     lcd.setCursor(13, 2);lcd.print(counter[7]*gear_rate); 
     lcd.setCursor(15, 3);lcd.print("Error"); 
 
    break; 

    case 6:
     lcd.init(); //初始化LCD
     lcd.backlight(); //開啟背光 
     lcd.setCursor(0, 0);lcd.print("1:"); 
     lcd.setCursor(2, 0);lcd.print(counter[1]*gear_rate);
     lcd.setCursor(0, 1);lcd.print("2:"); 
     lcd.setCursor(2, 1);lcd.print(counter[2]*gear_rate);  
     lcd.setCursor(0, 2);lcd.print("3:"); 
     lcd.setCursor(2, 2);lcd.print(counter[3]*gear_rate);  
     lcd.setCursor(0, 3);lcd.print("4:"); 
     lcd.setCursor(2, 3);lcd.print(counter[4]*gear_rate); 

     lcd.setCursor(11, 0);lcd.print("5:"); 
     lcd.setCursor(13, 0);lcd.print(counter[5]*gear_rate);
     lcd.setCursor(11, 1);lcd.print("6:"); 
     lcd.setCursor(13, 1);lcd.print(counter[6]*gear_rate);  
     lcd.setCursor(11, 2);lcd.print("7:"); 
     lcd.setCursor(13, 2);lcd.print(counter[7]*gear_rate); 
     lcd.setCursor(13, 3);lcd.print("Measure"); 
 
    break; 

    }
  }

}