#include <Wire.h>

#define GATE1 4
#define GATE2 5

#define DECODE(X) ((X ^ 0x17) + 0x17)

uint8_t nunchuckMin[2], nunchuckMax[2];

uint8_t nunchuck[6];
int count;
int state = LOW;
bool printflag = false;

void setup()
{
  nunchuckInit();

  digitalWrite(GATE1, HIGH);
  digitalWrite(GATE2, LOW);
  
  Serial1.begin(9600);

  for(int i=0; i<2; i++)
  {
    nunchuckMin[i] = 255;
    nunchuckMax[i] = 0;
  }
}

void loop()
{
  // ヌンチャク1
  Serial1.println("ヌンチャク1の計測");
  Serial1.println("Cボタンを押して開始");
  Serial1.println("終了はステイックから手を離してZボタン");
  do
  {
    requestData();
    sendZero();
    delay(100);
  }
  while(nunchuck[5] & 2);

  do
  {
    calibration();
    sendZero();
    delay(10);
  }
  while(nunchuck[5] & 1);

  requestData();
  Serial1.print("Xセンター: ");
  Serial1.print(nunchuck[0]);
  Serial1.print(" Yセンター: ");
  Serial1.println(nunchuck[1]);
  Serial1.println();

  changeNunchuck();
  sendZero();
  delay(10);

  // ヌンチャク2
  Serial1.println("ヌンチャク2の計測");
  Serial1.println("Cボタンを押して開始");
  Serial1.println("終了はステイックから手を離してZボタン");
  do
  {
    requestData();
    sendZero();
    delay(100);
  }
  while(nunchuck[5] & 2);

  do
  {
    calibration();
    sendZero();
    delay(10);
  }
  while(nunchuck[5] & 1);

  requestData();
  Serial1.print("Xセンター: ");
  Serial1.print(nunchuck[0]);
  Serial1.print(" Yセンター: ");
  Serial1.println(nunchuck[1]);

  Serial1.println("完了");
  while(true);
}

void requestData()
{
  count = 0;
  
  Wire.requestFrom(0x52, 6);
  while(Wire.available())
  {
    nunchuck[count] = DECODE(Wire.read());
    count++;
  }
}

void calibration()
{
  requestData();

  if(count >= 5)
  {
    for(int i=0; i<2; i++)
    {
      if(nunchuck[i] < nunchuckMin[i])
      {
        nunchuckMin[i] = nunchuck[i];
        printflag = true;
      }
      if(nunchuck[i] > nunchuckMax[i])
      {
        nunchuckMax[i] = nunchuck[i];
        printflag = true;
      }
    }

    if(printflag)
    {
      Serial1.print("X最小: ");
      Serial1.print(nunchuckMin[0]);
      Serial1.print(" X最大: ");
      Serial1.print(nunchuckMax[0]);
      Serial1.print(" Y最小: ");
      Serial1.print(nunchuckMin[1]);
      Serial1.print(" Y最大: ");
      Serial1.println(nunchuckMax[1]);
      printflag = false;
    }
  }
}

void changeNunchuck()
{
  if (state == LOW)
  {
    digitalWrite(GATE1, LOW);
    digitalWrite(GATE2, HIGH);
    state = HIGH;
  }
  else
  {
    digitalWrite(GATE1, HIGH);
    digitalWrite(GATE2, LOW);
    state = LOW;
  }
}

void sendZero()
{
  Wire.beginTransmission(0x52);
  Wire.write(0x00);
  Wire.endTransmission();
}

void nunchuckInit()
{
  pinMode(GATE1, OUTPUT);
  pinMode(GATE2, OUTPUT);
  
  digitalWrite(GATE1, HIGH);
  digitalWrite(GATE2, HIGH);

  Wire.begin();
  Wire.beginTransmission(0x52);
  Wire.write((uint8_t)0x40);
  Wire.write((uint8_t)0x00);
  Wire.endTransmission();
}