/*
  GLCD Library - Hello World
 
 This sketch prints "Hello World!" to the LCD
 and shows the time in seconds since reset.
 
 The circuit:
 See the inlcuded documentation in glcd/doc directory for how to wire
 up the glcd module. glcd/doc/GLCDref.htm can be viewed in your browser
 by clicking on the file.
 
 */

// include the library header
#include "Arduino.h"
#include "hexapod.h"
#include "glcd.h"
//#include "math.h"
//#include "Wire.h"
//#include "WiiChuck.h"
#include "bitmaps/allBitmaps.h"
//#include "IRremote.h"

#define MAXANGLE 90
#define MINANGLE -90

// include the Fonts
#include "fonts/allFonts.h"

WiiChuck chuck = WiiChuck();
int angleStart, currentAngle;
int tillerStart = 0;
double angle;
int RECV_PIN = 11;
int m, mt; //mode
long int oud, oud2, oudt, t; //ouden ontvangen code
String protocol;

IRrecv irrecv(RECV_PIN);

decode_results results;

void setup() {
  // Initialize the GLCD 
  GLCD.Init();

  pinMode(13, OUTPUT);
  digitalWrite(13, LOW);

  // Select the font for the default text area
  GLCD.SelectFont(SystemFont5x7);

  Serial.begin(9600);
  irrecv.enableIRIn(); // Start the IR receiver

  chuck.begin();
  while (!chuck.plugd)
    chuck.CheckifPlugd();

  //GLCD.DrawBitmap(Earth, 0, 0, WHITE);

  GLCD.CursorTo(12, 1); 
  GLCD.print("Nederland");
  delay(3500);
}

long int getIR(decode_results *results) {
  return results->value; 
}

void dump(decode_results *results) {
  int count = results->rawlen;
  GLCD.CursorTo(0, 1);
  if (results->decode_type == UNKNOWN) {
    protocol = "Unknown encoding: ";
  } 
  else if (results->decode_type == NEC) {
    protocol = "Decoded NEC: ";
  } 
  else if (results->decode_type == SONY) {
    protocol = "Decoded SONY: ";
  } 
  else if (results->decode_type == RC5) {
    protocol = "Decoded RC5: ";
  } 
  else if (results->decode_type == RC6) {
    protocol = "Decoded RC6: ";
  }
  GLCD.print(protocol);
  GLCD.CursorTo(0, 2); 
  GLCD.print(oud2, HEX); 
  GLCD.CursorTo(0, 3);
  GLCD.print(oud, HEX);
  GLCD.CursorTo(0, 4);
  long int t = results->value;
  oudt = t;
}

#define s1 50
#define s2 50
int stop = 0;
int stopstate = 0;

void loop() 
{
  if (irrecv.decode(&results)){
    long int t2 = getIR(&results);
    t = t2;
  }

  if (stop)
  {
    if ((t == 0x1045B) & (stopstate != 2)) {
      mt = 0;
      if (m == 0)
        m = 1;
      else
        m = 0;
      stopstate = 2;
    }
    else if (( t == 0x45B) & (stopstate != 3)) {
      mt = 0;
      if (m == 0)
        m = 1;
      else
        m = 0;
      stopstate = 3;
    }
    else if ((t == 0x1042C ) || ( t == 0x42C)) {
      stop = false;
    }
  }
  else if ((t == 0x431 ) || ( t == 0x10431)) { 
    digitalWrite(13, HIGH);
    stop = true;
  }
  else if ((t == 0x40F ) || ( t == 0x1040F)) {
    digitalWrite(13, HIGH);
    irrecv.resume(); // Receive the next value
    m = 60;
  }
  else if ((t == 0x1040C ) || ( t == 0x40C)) {
    //power
    GLCD.ClearScreen();
    //GLCD.DrawBitmap(Power, 64, 0, BLACK);

    while (1){
    }
  }
  else
  {
    mt++;
    digitalWrite(13, LOW);
  }

  switch (mt){
  case 1:   
    m = 0;  
    break;
  case s1:  
    m = 1;  
    break;
  case s1+s2:
    mt = 0; 
    break;
  }

  if (m == 0)
  {
    GLCD.ClearScreen();
    GLCD.CursorTo(0, 0); 
    GLCD.print("IR Receiver");
    //GLCD.DrawBitmap(Ir, 116, 0, BLACK);
    if (irrecv.decode(&results)) {
      dump(&results);
      delay(100);
      irrecv.resume(); // Receive the next value
      oud2 = oud;
    }
    else
    {
      oud = oudt;
      GLCD.CursorTo(0, 1);
      GLCD.print(protocol);
      GLCD.CursorTo(0, 2); 
      GLCD.print(oud2, HEX); 
      GLCD.CursorTo(0, 3); 
      GLCD.print(oud, HEX); 
      GLCD.CursorTo(0, 4); 
      GLCD.print("No DATA Input"); 
    }

  }
  else
  {
    GLCD.ClearScreen();

    chuck.update();
    if (!chuck.plugd)
      chuck.CheckifPlugd();
    else {

      GLCD.CursorTo(0, 0);

      GLCD.print("MangaValk");
      //GLCD.DrawBitmap(Nun, 106, 0, BLACK);

      GLCD.CursorTo(0, 2);

      GLCD.print("C: "); 
      GLCD.print(chuck.buttonC); 
      GLCD.print(" Z: "); 
      GLCD.print(chuck.buttonZ);

      GLCD.CursorTo(0, 3);
      GLCD.print("Roll: ");
      GLCD.print(chuck.readRoll());
      GLCD.CursorTo(0, 4);
      GLCD.print("Pitch: ");
      GLCD.print(chuck.readPitch()-75);
      GLCD.CursorTo(0, 5);
      GLCD.print("Jy: ");
      GLCD.print(chuck.readJoyY());
      GLCD.CursorTo(0, 6);
      GLCD.print("Jx: ");
      GLCD.print(chuck.readJoyX()); 
      GLCD.CursorTo(0, 7);
      GLCD.print("Atan2: ");
      GLCD.print(atan2(chuck.readJoyX(), chuck.readJoyY()));
    }

    irrecv.resume(); // Receive the next value
  }
  delay(100);
}