// Version 1

#include "Arduino.h"
#include "CustomTimer.h"
#include "HexapodClass.h"
#include "Vector.h"
#include "Global.h"

//#include <avr/wdt.h>

void setup()
{
  //wdt_enable(WDTO_1S);

  walk1[0] = Vector(12, robotHeigth, 2);
  walk1[1] = Vector(12, robotHeigth, 0);
  walk1[2] = Vector(12, robotHeigth, -2);
  walk1[3] = Vector(12, robotHeigth, -2);
  walk1[4] = Vector(12, robotHeigth - 10, -2);
  walk1[5] = Vector(12, robotHeigth - 10, 0);
  walk1[6] = Vector(12, robotHeigth - 10, 2);
  walk1[7] = Vector(12, robotHeigth, 2);

  Serial.begin(115200);
  Serial1.begin(115200);

  Serial.println("");
  Serial.println(" ____                     ____    ___                ");
  Serial.println("/\\  _`\\   __             /\\  _`\\ /\\_ \\               ");
  Serial.println("\\ \\ \\L\\_\\/\\_\\  _ __    __\\ \\ \\L\\_\\//\\ \\    __  __    ");
  Serial.println(" \\ \\  _\\/\\/\\ \\/\\`'__\\/'__`\\ \\  _\\/ \\ \\ \\  /\\ \\/\\ \\   ");
  Serial.println("  \\ \\ \\/  \\ \\ \\ \\ \\//\\  __/\\ \\ \\/   \\_\\ \\_\\ \\ \\_\\ \\  ");
  Serial.println("   \\ \\_\\   \\ \\_\\ \\_\\\\ \\____\\\\ \\_\\   /\\____\\\\/`____ \\ ");
  Serial.println("    \\/_/    \\/_/\\/_/ \\/____/ \\/_/   \\/____/ `/___/> \\");
  Serial.println("                                               /\\___/");
  Serial.println("                                               \\/__/ ");
  Serial.println("");
  Serial.println("Version 3.0");
  delay(800);
}

void loop()
{
  // the program is alive...for now.
  //wdt_reset();

  //  Rotation calculation, please use transformation matrixes instead.
  //rotationX = ( counterXRotation % (rotationRange * 4) < (rotationRange * 2) ? -rotationRange + counterXRotation % (rotationRange * 2) : rotationRange + (-1 * (counterXRotation % (rotationRange * 2))) );
  //rotationY = ( counterYRotation % (rotationRange * 4) < (rotationRange * 2) ? -rotationRange + counterYRotation % (rotationRange * 2) : rotationRange + (-1 * (counterYRotation % (rotationRange * 2))) );
  //rotationZ = ( counterZRotation % (rotationRange * 4) < (rotationRange * 2) ? -rotationRange + counterZRotation % (rotationRange * 2) : rotationRange + (-1 * (counterZRotation % (rotationRange * 2))) );

  DrawBody();
}