// hexapod.cpp

#ifndef HEX
#define HEX

#include "Arduino.h"
#include "hexapod.h"

void hexapod::FireFly()
{
  Serial.println("");
  Serial.println(" ____                     ____    ___                ");
  Serial.println("/\\  _`\\   __             /\\  _`\\ /\\_ \\               ");
  Serial.println("\\ \\ \\__  /\\_\\  _ __    __\\ \\ \\__  //\\ \\    __  __    ");
  Serial.println(" \\ \\  _\\ \\/\\ \\/\\`'__\\/'__`\\ \\  _\\  \\ \\ \\  /\\ \\/\\ \\   ");
  Serial.println("  \\ \\ \\/  \\ \\ \\ \\ \\//\\  __/\\ \\ \\/   \\_\\ \\_\\ \\ \\_\\ \\  ");
  Serial.println("   \\ \\_\\   \\ \\_\\ \\_\\\\ \\____\\\\ \\_\\   /\\____\\\\/`____ \\ ");
  Serial.println("    \\/_/    \\/_/\\/_/ \\/____/ \\/_/   \\/____/ `/___/> \\");
  Serial.println("                                               /\\___/");
  Serial.println("                                               \\/__/");
  Serial.println("");
  Serial.println("Version 3.1");
  delay(800);
  irrecv = new IRrecv((int)5);
  irrecv->enableIRIn();
}

int hexapod::GetNewCmd()
{
  if (irrecv->decode(&results)) {
    if (results.value != old.value){
      if (results.decode_type == RC6) {
	  #ifdef DEBUG
        Serial.print("### GetNewCmd->Decoded RC6: ");
        Serial.println(results.value, HEX);
		#endif
        old.value = results.value;
        rr = false;
        s = true;
      }
      else if (rr != true)
      {
	  #ifdef DEBUG
        Serial.println("### GetNewCmd->Not RC6");
		#endif
        rr = true;
      }
    }
    irrecv->resume(); // Receive the next value
  }

  if (s)
  {
    s = false;
    return results.value;
  }
  else return 0;
}

float microsecondsToCentimeters(float microseconds)
{
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return microseconds / 29 / 2;
}

float hexapod::GetUltraSoneDistance()
{
  float duration, cm;

  pinMode(PingS, OUTPUT);
  digitalWrite(PingS, LOW);
  delayMicroseconds(2);
  digitalWrite(PingS, HIGH);
  delayMicroseconds(5);
  digitalWrite(PingS, LOW);

  pinMode(PingR, INPUT);
  duration = pulseIn(PingR, HIGH);
  
  // convert the time into a distance
  cm = microsecondsToCentimeters(duration);
  
  return cm;
}

void hexapod::CoreSend(DataObject *Input)
{
	//Init
	Converter Con;
	InverseKinemetics IK;
	Servos SSC32;
	Data *DataLine = new Data;

	//Main
	DataLine->input(Input);
	DataLine->WalkTime = 1000;

	IK.IK(DataLine);
    Con.Convert(DataLine);
    SSC32.Send(DataLine);
}

#endif