#include <LiquidCrystal_I2C.h>  // LCD I2C용 라이브러리
#include <Wire.h>   // I2C통신 라이브러리
#define JY_PIN A0
#define JX_PIN A1
#define JB_PIN 7

#define TSDIF 0.2   // Forward Turn Speed Difference
#define SSRAT 0.5   // Spin-turn Speed Ratio

LiquidCrystal_I2C lcd(0x27,20,4); // 접근주소: 0x3F or 0x27

const char Onoff[2][4]={"OFF","ON"};
const char Mode[11][20]={"START","FORWARD","FORWARD RIGHT","SPIN-TURN RIGHT","BACKWARD RIGHT",
    "BACKWARD","BACKWARD LEFT","SPIN-TURN LEFT","FORWARD LEFT","STOP"};
char buf[25]="";
int xPos=0,yPos=0,bState=0;
int leftMotor=0,rightMotor=0;
int xSpeed=0,ySpeed=0;
int speed=0,mode=0;

void setup() {
  pinMode(JB_PIN,INPUT);
  lcd.init();   // LCD 초기화
  lcd.backlight();  // LCD 백라이트 켜기
}

void loop() {
  xPos=1023-analogRead(JX_PIN);
  yPos=analogRead(JY_PIN);
  bState=digitalRead(JB_PIN);

  speed=155+100*bState;   // Max Speed Setting

  if (yPos>412 && yPos<611) {
    if (xPos>=611) { 
      mode=3;
      leftMotor=map(xPos,611,1023,0,speed*SSRAT);
      rightMotor=map(xPos,611,1023,0,-speed*SSRAT);
    }
    else if (xPos<=412) { 
      mode=7; 
      leftMotor=map(xPos,0,412,-speed*SSRAT,0);
      rightMotor=map(xPos,0,412,speed*SSRAT,0);
    }
    else {
      mode=9;
      leftMotor=0;
      rightMotor=0;
    }
  }
  else if (yPos>=611) {
    if (xPos>=611) { 
      mode=2; xSpeed=map(xPos,611,1023,0,speed*TSDIF); ySpeed=map(yPos,611,1023,0,speed);
      leftMotor=ySpeed;
      rightMotor=ySpeed-xSpeed;
    }
    else if (xPos<=412) { 
      mode=8; xSpeed=map(xPos,0,412,speed*TSDIF,0); ySpeed=map(yPos,611,1023,0,speed);
      leftMotor=ySpeed-xSpeed;
      rightMotor=ySpeed;
    }
    else {
      mode=1;
      leftMotor=rightMotor=map(yPos,611,1023,0,speed);
    }
  }
  else {
    if (xPos>=611) { 
      mode=4; xSpeed=map(xPos,611,1023,0,speed*TSDIF); ySpeed=map(yPos,0,412,-speed,0);
      leftMotor=ySpeed;
      rightMotor=ySpeed+xSpeed;
    }
    else if (xPos<=412) { 
      mode=6; xSpeed=map(xPos,0,412,speed*TSDIF,0); ySpeed=map(yPos,0,412,-speed,0);
      leftMotor=ySpeed+xSpeed;
      rightMotor=ySpeed;
    }
    else {
      mode=5;
      leftMotor=rightMotor=map(yPos,0,412,-speed,0);
    }
  }

  displayLCD();
  delay(50);
}



void displayLCD()
{
  sprintf(buf,"X:%4d Y:%4d  B:%3s",xPos,yPos,Onoff[bState]);
  lcd.setCursor(0,0); lcd.print(buf);
  sprintf(buf,"%-15s",Mode[mode]);
  lcd.setCursor(0,1); lcd.print("MODE:"); lcd.print(buf);
  lcd.setCursor(0,2); lcd.print("LFT MOTOR  RHT MOTOR");
  sprintf(buf,"   %3d       %3d    ",leftMotor,rightMotor);
  lcd.setCursor(0,3); lcd.print(buf);
}