#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

Servo base;
Servo catcher;
WiiNun WiiNun;
Motor horMotor;
Motor verMotor;

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にする
  
  static byte baseDeg = 90;
  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*2);
    baseDeg += x/32; // 台座をxに応じて相対移動
  }
  base.write(baseDeg);
  
  if(0);
  else if(ay > +0x20) verMotor.mode(GO);
  else if(ay > +0x10) verMotor.speed(+0xCC);
  else if(ay < -0x20) verMotor.mode(BACK);
  else if(ay < -0x10) verMotor.speed(-0xCC);
  else verMotor.mode(STOP);
  
  byte catcherDeg = 135;
  if(cbut){  // cボタン押すとキャッチャーを離す
    catcherDeg = 45;
  }
  catcher.write(catcherDeg);
  
  delay(50);
}