/*  USB328_RCSW_Servo2.ino
    2018 Programmed by gijin77@gmail.com
*/
#include <Wire.h>
#include <ACROBOTIC_SSD1306.h>
#include <RCSwitch.h>
#include <Servo.h>

RCSwitch mySwitch = RCSwitch();
Servo myServoA;  // create a servo object
Servo myServoB;  // create a servo object
Servo myServoC;  // create a servo object

#define LED 13
#define ON  HIGH
#define OFF LOW
#define LED_A 10
#define LED_B 11
#define LED_C 12

#define L_SWA 11481016
#define L_SWB 11481012
#define L_SWC 11481010
#define L_SWD 11481009

int sva_90=100; //サーボA 90度offset
int svb_90=100; //サーボB 90度offset
int svc_90=100; //サーボC 90度offset
int sva_on=24;  //照明ON時offset
int sva_off=18; //照明OFF時offset
int svb_on=22;  //照明ON時offset
int svb_off=16; //照明OFF時offset
int svc_on=26;  //照明ON時offset
int svc_off=26; //照明OFF時offset

long unsigned int rxdat  = 0; //433MHz受信データ
int rxlen;
int rxprt;
int angle; //サーボモータ角度
int sw_a=0; //照明1の状態
int sw_b=0; //照明2の状態
int sw_c=0; //照明3の状態
int sw_d=0; //全体制御の状態
int stup=0;

void setup() {
  Serial.begin(115200);
  Serial.println("Program Start");

  pinMode(LED_A, OUTPUT);
  pinMode(LED_B, OUTPUT);
  pinMode(LED_C, OUTPUT);
  pinMode(LED, OUTPUT);

  myServoA.attach(A0);      //Servo pin=A0
  myServoB.attach(A1);      //Servo pin=A1
  myServoC.attach(A2);      //Servo pin=A2
  myServoA.write(sva_90);
  myServoB.write(svb_90);
  myServoC.write(svc_90);

  Wire.begin(); 
  
//mySwitch.enableReceive(0);  // Receiver on interrupt 0 => that is pin #2
  mySwitch.enableReceive(1);  // Receiver on interrupt 1 => that is pin #3

  oled.init();               // Initialze SSD1306 OLED display

//8x16font  
  oled.clearDisplay();    
  oled.putString16(0,0,"433Mhz Remocon");
  oled.putString16(2,0,"Servo Control");
  oled.putString16(4,0,"OLED  Display");
  oled.putString16(6,0,"2018/11/11 by YH");
  for(int i=0;i<3;i++) {
    digitalWrite(LED_A,ON);delay(200);digitalWrite(LED_A,OFF);
    digitalWrite(LED_B,ON);delay(200);digitalWrite(LED_B,OFF);
    digitalWrite(LED_C,ON);delay(200);digitalWrite(LED_C,OFF);
    digitalWrite(LED,ON);delay(200);digitalWrite(LED,OFF);
  }
//test text 8x8font
  oled.clearDisplay();              // Clear screen
  oled.setTextXY(0,0);oled.putString("1234567890123456");
  oled.setTextXY(0,0);oled.putString("USB328P  433Mhz ");
  oled.setTextXY(1,0);oled.putString(" D+ =D2  D- =D4 ");
  oled.setTextXY(2,0);oled.putString("SvoA=A0 LEDA=D10");
  oled.setTextXY(3,0);oled.putString("SvoB=A1 LEDB=D11");
  oled.setTextXY(4,0);oled.putString("SvoC=A2 LEDC=D12");
  oled.setTextXY(5,0);oled.putString("SDA =A4 LED =D13");
  oled.setTextXY(6,0);oled.putString("SCL =A5 Rxin=D3 ");
  oled.setTextXY(7,0);oled.putString("2018/11/11 by YH");

  for(int i=0;i<3;i++) {
    digitalWrite(LED_A,ON);
    digitalWrite(LED_B,ON);
    digitalWrite(LED_C,ON);
    delay(200);
    digitalWrite(LED_A,OFF);
    digitalWrite(LED_B,OFF);
    digitalWrite(LED_C,OFF);
    delay(200);
  }
}
void sw_a_on(){
  digitalWrite(LED_A,ON);
  myServoA.write(sva_90+sva_on); //A-UP
  delay(300);
  myServoA.write(sva_90);
  oled.putString16(2,0,"           "); 
  oled.putString16(2,0,"  SW_A = " + String(sw_a)); 
}
void sw_a_off(){
  digitalWrite(LED_A,OFF);
  myServoB.write(svb_90-sva_off); //B-UP
  delay(300);
  myServoB.write(svb_90);
  oled.putString16(2,0,"           "); 
  oled.putString16(2,0,"  SW_A = " + String(sw_a)); 
}
void sw_b_on(){
  digitalWrite(LED_B,ON);
  myServoA.write(sva_90-svb_off); //A->DWN
  delay(300);
  myServoA.write(sva_90);
  oled.putString16(4,0,"           "); 
  oled.putString16(4,0,"  SW_B = " + String(sw_b));  
}
void sw_b_off(){
  digitalWrite(LED_B,OFF);
  myServoB.write(svb_90+svb_off); //B->DWN
  delay(300);
  myServoB.write(svb_90);
  oled.putString16(4,0,"           "); 
  oled.putString16(4,0,"  SW_B = " + String(sw_b));    
}
void sw_c_on(){
  digitalWrite(LED_C,ON);
  myServoC.write(svc_90+svc_on);
  delay(300);
  myServoC.write(svc_90);
  oled.putString16(6,0,"           "); 
  oled.putString16(6,0,"  SW_C = " + String(sw_c));            
}
void sw_c_off(){
  digitalWrite(LED_C,OFF);
  myServoC.write(svc_90-svc_off);  
  delay(300);
  myServoC.write(svc_90);
  oled.putString16(6,0,"           "); 
  oled.putString16(6,0,"  SW_C = " + String(sw_c));        
}

void loop() {
  if (mySwitch.available()) {
    if (stup==0) {
      oled.clearDisplay();
      oled.putString16(2,0,"  SW_A = " + String(sw_a)); 
      oled.putString16(4,0,"  SW_B = " + String(sw_b));  
      oled.putString16(6,0,"  SW_C = " + String(sw_c));        
      stup=1;   
    }
    digitalWrite(LED,ON);
    rxdat=mySwitch.getReceivedValue();
    rxlen=mySwitch.getReceivedBitlength();
    rxprt=mySwitch.getReceivedProtocol();
    oled.putString16(0,0,"RxD=            ");
    oled.putString16(0,0,"RxD=" + String(rxdat));
    
    if (rxdat==L_SWA) {
        if (sw_a==0) {
          sw_a=1;sw_a_on();
        } else {
          sw_a=0;sw_a_off();
        }
    } else if (rxdat==L_SWB) {
        if (sw_b==0) {
          sw_b=1;sw_b_on();
        } else {
          sw_b=0;sw_b_off();
        }
    } else if (rxdat==L_SWC) {
        if (sw_c==0) {
          sw_c=1;sw_c_on();

        } else {
          sw_c=0;sw_c_off();
        }
    } else if (rxdat==L_SWD) {
      sw_d=sw_a+sw_b+sw_c;
      if (sw_d<2) {
          sw_a=sw_b=sw_c=1;
          sw_a_on();delay(500);sw_b_on();delay(500);sw_c_on();
      } else {
          sw_a=sw_b=sw_c=0;
          sw_a_off();delay(500);sw_b_off();delay(500);sw_c_off();
      }
    }
    delay(100);
    digitalWrite(LED,OFF);
      
    Serial.print("Received ");
    Serial.print( rxdat );
    Serial.print(" / ");
    Serial.print( rxlen );
    Serial.print("bit ");
    Serial.print("Protocol: ");
    Serial.println( rxprt );

    mySwitch.resetAvailable();
    
 }
 delay(20);
}