// Wii_OLED_Test
// Copyright (C)2015 Rinky-Dink Electronics, Henning Karlsen. All right reserved
// web: http://www.RinkyDinkElectronics.com/
//
// An example demonstrating how to use my OLED_I2C and WiiChuck libraries to 
// rotate two 3D vector planes based on input from the nunchuck.
//

#include <OLED_I2C.h>
#include <WiiChuck.h>

OLED  myOLED(SDA, SCL);
WiiChuck myChuck(SDA, SCL);

extern uint8_t SmallFont[];

double vectors[2][8][3] = {{{10, 10, 10},{-10, 10, 10},{-10, -10, 10},{10, -10, 10}},{{10, 10, 10},{-10, 10, 10},{-10, -10, 10},{10, -10, 10}}};
double temp[2][8][3];

double perspective = 100.0f;
int deltaX1, deltaY1, deltaX2, deltaY2;

void setup()
{
  myOLED.begin();
  myOLED.setFont(SmallFont);

  myChuck.begin();
}

void loop()
{
  myChuck.readData();

  myOLED.clrScr();
  myOLED.print("Accel.", LEFT, 0);
  myOLED.print("Joystick", RIGHT, 0);
  myOLED.drawLine(64, 0, 64, 54);
  myOLED.drawLine(0, 54, 127, 54);
  myOLED.print("Buttons:", LEFT, 56);
  myOLED.setPixel(29, 32);
  myOLED.setPixel(99, 32);
  drawVectors();

  for (int c1 = 0; c1 < 2; c1++)
    for (int c2 = 0; c2 < 8; c2++)
      for (int c3 = 0; c3 < 3; c3++)
        temp[c1][c2][c3] = vectors[c1][c2][c3];
  
  deltaX1 = myChuck.getPitchAngle();
  deltaY1 = -myChuck.getRollAngle();

  deltaX2 = (int)((float)myChuck.getJoyY() * 0.9f);
  deltaY2 = (int)(-(float)myChuck.getJoyX() * 0.9f);

  rotateX(deltaX1, 0);
  rotateY(deltaY1, 0);
  rotateX(deltaX2, 1);
  rotateY(deltaY2, 1);

  if (myChuck.checkButtonC())
    myOLED.print("C", 56, 56);
  else
    myOLED.print("-", 56, 56);
  if (myChuck.checkButtonZ())
    myOLED.print("Z", 66, 56);
  else
    myOLED.print("-", 66, 56);

  myOLED.update();
}

int translateX(double x, double z, int offset = 0)
{
  return (int)((x + (64 + offset)) + (z * (x / perspective)));
}

int translateY(double y, double z, int offset = 0)
{
  return (int)((y + 32) + (z * (y / perspective)));
}

void rotateX(int angle, int array)
{
  double rad, cosa, sina, Yn, Zn;
 
  rad = angle * PI / 180;
  cosa = cos(rad);
  sina = sin(rad);
  for (int i = 0; i < 8; i++)
  {
    Yn = (temp[array][i][1] * cosa) - (temp[array][i][2] * sina);
    Zn = (temp[array][i][1] * sina) + (temp[array][i][2] * cosa);
    temp[array][i][1] = Yn;
    temp[array][i][2] = Zn;
  }
}

void rotateY(int angle, int array)
{
  double rad, cosa, sina, Xn, Zn;
 
  rad = angle * PI / 180;
  cosa = cos(rad);
  sina = sin(rad);
  for (int i = 0; i < 8; i++)
  {
    Xn = (temp[array][i][0] * cosa) - (temp[array][i][2] * sina);
    Zn = (temp[array][i][0] * sina) + (temp[array][i][2] * cosa);
    temp[array][i][0] = Xn;
    temp[array][i][2] = Zn;
  }
}

void drawVectors()
{
  myOLED.drawLine(translateX(temp[0][0][0], temp[0][0][2], -35), translateY(temp[0][0][1], temp[0][0][2]), translateX(temp[0][1][0], temp[0][1][2], -35), translateY(temp[0][1][1], temp[0][1][2]));
  myOLED.drawLine(translateX(temp[0][1][0], temp[0][1][2], -35), translateY(temp[0][1][1], temp[0][1][2]), translateX(temp[0][2][0], temp[0][2][2], -35), translateY(temp[0][2][1], temp[0][2][2]));
  myOLED.drawLine(translateX(temp[0][2][0], temp[0][2][2], -35), translateY(temp[0][2][1], temp[0][2][2]), translateX(temp[0][3][0], temp[0][3][2], -35), translateY(temp[0][3][1], temp[0][3][2]));
  myOLED.drawLine(translateX(temp[0][3][0], temp[0][3][2], -35), translateY(temp[0][3][1], temp[0][3][2]), translateX(temp[0][0][0], temp[0][0][2], -35), translateY(temp[0][0][1], temp[0][0][2]));
  myOLED.drawLine(translateX(temp[0][0][0], temp[0][0][2], -35), translateY(temp[0][0][1], temp[0][0][2]), translateX(temp[0][2][0], temp[0][2][2], -35), translateY(temp[0][2][1], temp[0][2][2]));
  myOLED.drawLine(translateX(temp[0][1][0], temp[0][1][2], -35), translateY(temp[0][1][1], temp[0][1][2]), translateX(temp[0][3][0], temp[0][3][2], -35), translateY(temp[0][3][1], temp[0][3][2]));

  myOLED.drawLine(translateX(temp[1][0][0], temp[1][0][2], 35), translateY(temp[1][0][1], temp[1][0][2]), translateX(temp[1][1][0], temp[1][1][2], 35), translateY(temp[1][1][1], temp[1][1][2]));
  myOLED.drawLine(translateX(temp[1][1][0], temp[1][1][2], 35), translateY(temp[1][1][1], temp[1][1][2]), translateX(temp[1][2][0], temp[1][2][2], 35), translateY(temp[1][2][1], temp[1][2][2]));
  myOLED.drawLine(translateX(temp[1][2][0], temp[1][2][2], 35), translateY(temp[1][2][1], temp[1][2][2]), translateX(temp[1][3][0], temp[1][3][2], 35), translateY(temp[1][3][1], temp[1][3][2]));
  myOLED.drawLine(translateX(temp[1][3][0], temp[1][3][2], 35), translateY(temp[1][3][1], temp[1][3][2]), translateX(temp[1][0][0], temp[1][0][2], 35), translateY(temp[1][0][1], temp[1][0][2]));
  myOLED.drawLine(translateX(temp[1][0][0], temp[1][0][2], 35), translateY(temp[1][0][1], temp[1][0][2]), translateX(temp[1][2][0], temp[1][2][2], 35), translateY(temp[1][2][1], temp[1][2][2]));
  myOLED.drawLine(translateX(temp[1][1][0], temp[1][1][2], 35), translateY(temp[1][1][1], temp[1][1][2]), translateX(temp[1][3][0], temp[1][3][2], 35), translateY(temp[1][3][1], temp[1][3][2]));
}