#include <NintendoExtensionCtrl.h>
#include <Servo.h>

/*
Connect board VIN (red wire) to 3.3v
Connect board GND (black wire) to GND
Connect board SCL (yellow wire) Clock to SCL on UNO or A5
Connect board SDA (blue wire) Data to SDA on UNO or A4
*/

Nunchuk nchuk;
Servo xServo;  // Create servo object for X axis
Servo yServo;  // Create servo object for Y axis

void setup() {
    Serial.begin(115200);
    nchuk.begin();

    xServo.attach(9);
    yServo.attach(10);

    while (!nchuk.connect()) {
        Serial.println("Nunchuk not detected!");
        delay(1000);
    }
}

void loop() {
    boolean success = nchuk.update();  // Get new data from the controller

    if (success == true) {
        nchuk.printDebug();  // Print all of the values

        // Get joystick values
        int xAxis = nchuk.joyX(); // Replace with actual method to get X axis value
        int yAxis = nchuk.joyY(); // Replace with actual method to get Y axis value

        // Map joystick values (0-255) to servo angle (0-180)
        int xServoAngle = map(xAxis, 0, 255, 0, 180);
        int yServoAngle = map(yAxis, 0, 255, 0, 180);

        // Set servo positions
        xServo.write(xServoAngle);
        yServo.write(yServoAngle);
    }
    else {  // Data is bad
        Serial.println("Controller Disconnected!");
        delay(1000);
        nchuk.connect();
    }
}