#include <Servo.h>
#include <Wire.h>
#include <WiiNun.h>
#include "Motor.h"

#define PIN_LED 13

#define PIN_MOTOR_HORIZON_1      5
#define PIN_MOTOR_HORIZON_2      4
#define PIN_MOTOR_VERTICAL_1     6
#define PIN_MOTOR_VERTICAL_2     7

#define PIN_SERVO_BASE         9
#define PIN_SERVO_CATCHER      10

#define PIN_PWR A3
#define PIN_GND A2

int loop_cnt = 0;
Servo base;
Servo catcher;
WiiNun wiiNun;
Motor horMotor;
Motor verMotor;

byte baseDeg = 90;
byte catcherDeg = 90;

void setup(){
  pinMode(PIN_PWR, OUTPUT);
  pinMode(PIN_GND, OUTPUT);
  digitalWrite(PIN_PWR, HIGH);
  digitalWrite(PIN_GND, LOW);
  delay(100);
//  Serial.begin(19200);
  wiiNun.begin();
  base.attach(PIN_SERVO_BASE);
  catcher.attach(PIN_SERVO_CATCHER);
  pinMode(PIN_LED , OUTPUT);
  horMotor.attach(PIN_MOTOR_HORIZON_1, PIN_MOTOR_HORIZON_2);
  verMotor.attach(PIN_MOTOR_VERTICAL_1, PIN_MOTOR_VERTICAL_2);
}

void loop(){
  // ヌンチャクを読む
  wiiNun.get();
  byte joyx = wiiNun.data[0];
  byte joyy = wiiNun.data[1];
  byte accx = wiiNun.data[2];
  byte accy = wiiNun.data[3];
  byte accz = wiiNun.data[4];
  byte zbut = ((~wiiNun.data[5] >> 0) & 1);
  byte cbut = ((~wiiNun.data[5] >> 1) & 1);
  if((wiiNun.data[5] >> 2) & 1) accx += 2;
  if((wiiNun.data[5] >> 3) & 1) accx += 1;
  if((wiiNun.data[5] >> 4) & 1) accy += 2;
  if((wiiNun.data[5] >> 5) & 1) accy += 1;
  if((wiiNun.data[5] >> 6) & 1) accz += 2;
  if((wiiNun.data[5] >> 7) & 1) accz += 1;
  // ヌンチャクおわり
  
  char x = (char)(joyx-0x7F);  // スティックの中心を0にする
  char y = (char)(joyy-0x7F);  // スティックの中心を0にする
//  char ax = (char)(accx-0x7F);
  char ay = (char)(accy-0x7F); // 水平状態を0にする
  
  if(zbut){  // zボタン押しながら
    digitalWrite(PIN_LED, HIGH);
    if(y>0x1F) baseDeg = 180 - atan2(y, x)*180/PI;  // 台座をスティックの角度にする
    if(baseDeg < 10) baseDeg = 10;
    else if(baseDeg > 170) baseDeg = 170;
  }else{  // zボタン離すと
    digitalWrite(PIN_LED, LOW);
    horMotor.speed(y);
    baseDeg += x/32; // 台座をxに応じて相対移動
  }
  
  if(0);
  else if(ay > +0x20) verMotor.mode(GO);
  else if(ay > +0x10) verMotor.speed(+0xCC);
  else if(ay < -0x10) verMotor.speed(-0xCC);
  else if(ay < -0x20) verMotor.mode(BACK);
  
  catcherDeg = 90;
  if(cbut){  // cボタン押すとキャッチャーを離す
    catcherDeg = 15;
  }
  
  base.write(baseDeg);
  catcher.write(catcherDeg);
  delay(10);
}