#include <DCMotor.h>
#include "wiring_private.h"
#include <avr/io.h>
#include <avr/power.h>


#if !defined(MCUCSR)
#define MCUCSR                  MCUSR
#endif

//watchdog registers to the same name (WDTCSR)
#if !defined(WDTCSR)
#define WDTCSR                  WDTCR
#endif

//if enhanced watchdog, define irq values, create disable macro
#if defined(WDIF)
#define WD_IRQ                  0xC0
#define WD_RST_IRQ              0xC8
#define WD_DISABLE()            do{                       \
    MCUCSR &= ~(1<<WDRF); \
    WD_SET(WD_OFF);       \
  }while(0)

#endif
#define WDTO_15MS               0
#define WD_SET(val,...)                                 \
  __asm__ __volatile__(                               \
      "in __tmp_reg__,__SREG__"           "\n\t"      \
      "cli"                               "\n\t"      \
      "wdr"                               "\n\t"      \
      "sts %[wdreg],%[wden]"              "\n\t"      \
      "sts %[wdreg],%[wdval]"             "\n\t"      \
      "out __SREG__,__tmp_reg__"          "\n\t"      \
      :                                               \
      : [wdreg] "M" (&WDTCSR),                        \
      [wden]  "r" ((uint8_t)(0x18)),                \
      [wdval] "r" ((uint8_t)(val|(__VA_ARGS__+0)))  \
      : "r0"                                          \
                      )


/* Multiplo Pyfirmata UNLP vars */
DCMotor motor0(M0_EN, M0_D0, M0_D1);
DCMotor motor1(M1_EN, M1_D0, M1_D1);

void setup() {
  // put your setup code here, to run once:
  motor1.setClockwise(false);
  Serial1.begin(57600);
  pinMode(3, OUTPUT);

  MCUSR &= ~(1 << WDRF);
  WD_SET(0);

  /* Disable clock division */
  clock_prescale_set(clock_div_1);



  sei();

  // BECAREFULL
  // Timer/Counter0, 1, and 3 share the same prescaler module, but the Timer/Counters can have different prescaler settings.

  //=====================================================================
  // Timer 0 Delay functions y OC0A como salida de PWM en pin 9
  //=====================================================================

  // on the ATmega168, timer 0 is also used for fast hardware pwm
  // (using phase-correct PWM would mean that timer 0 overflowed half as often
  // resulting in different millis() behavior on the ATmega8 and ATmega168)
  sbi(TCCR0A, WGM01);
  sbi(TCCR0A, WGM00);

  // set timer 0 prescale factor to 64
  cbi(TCCR0B, CS02);
  sbi(TCCR0B, CS01);
  sbi(TCCR0B, CS00);

  // enable timer 0 overflow interrupt
  sbi(TIMSK0, TOIE0);

  //----Configuracion para OC0A-----
  // El WGM0X ya esta definido por que lo usa arduino para el timer
  //sbi(TCCR0A,COM0A1);
  cbi(TCCR0A, COM0A0);
  cbi(TCCR0B, WGM02);


  //=====================================================================
  //Timer 3 PIN 6 only OC3A
  //=====================================================================

  // se libera el timer3 para compatibilizar el core con la RGLib
  // (se pierde la funcion PWM del PIN6)

  //OC4D pin connected
  //nOC4D pin Disconnected
  //sbi(TCCR3A,COM3A1);
  //cbi(TCCR3A,COM3A0);

  //cbi(TCCR3B, WGM33);
  //cbi(TCCR3B, WGM32);
  //cbi(TCCR3A, WGM31);
  //sbi(TCCR3A, WGM30);

  //En ppo esta apagado el pwm
  //cbi(TCCR3B,CS43);
  //cbi(TCCR3B,CS42);
  //cbi(TCCR3B,CS41);
  //sbi(TCCR3B,CS40);

  //=====================================================================
  //Timer 4 Motors PWM (A & B) y GP TIMER D
  //=====================================================================

  //OC4A pin connected
  //nOC4A pin Disconnected
  //sbi(TCCR4A,COM4A1);
  cbi(TCCR4A, COM4A0);

  //OC4B pin connected
  //nOC4B pin Disconnected
  //sbi(TCCR4A,COM4B1);
  cbi(TCCR4A, COM4B0);

  //Enable PWM en A y B
  //Phase and Frequency Correct PWM
  sbi(TCCR4A, PWM4A);
  sbi(TCCR4A, PWM4B);

  //Prescaler --> Freq sea 31.35Khz.

  //En ppo esta apagado el pwm
  cbi(TCCR4B, CS43);
  cbi(TCCR4B, CS42);
  cbi(TCCR4B, CS41);
  sbi(TCCR4B, CS40);

  //--------------------------------------------------------------------------------------
  // Timer 4D para el pin 5

  //OC4D pin connected
  //nOC4D pin Disconnected
  //sbi(TCCR4D,COM4D1);
  cbi(TCCR4D, COM4D0);

  sbi(TCCR4C, PWM4D);

  //Phase and Frequency Correct PWM
  cbi(TCCR4D, WGM41);
  sbi(TCCR4D, WGM40);

  //=====================================================================
  // A/D
  //=====================================================================

  // set a2d prescale factor to 128
  // 16 MHz / 128 = 125 KHz, inside the desired 50-200 KHz range.
  // XXX: this will not work properly for other clock speeds, and
  // this code should use F_CPU to determine the prescale factor.
  sbi(ADCSRA, ADPS2);
  sbi(ADCSRA, ADPS1);
  sbi(ADCSRA, ADPS0);

  // enable a2d conversions
  sbi(ADCSRA, ADEN);


  //=====================================================================
  //  USART
  //=====================================================================
  // We dont need to do anything


  //=====================================================================
  //  Watchdog de CDC
  //=====================================================================

  // The watchdog timer is configured for interrupting every 15ms for the cdc and usb tasks.
  WD_SET(WD_IRQ, WDTO_15MS);



}

int a4, a5;

void loop() {
  // put your main code here, to run repeatedly:
  a4 = analogRead(4);
  a5 = analogRead(5);
  Serial1.print("A4 = ");
  Serial1.println(a4);
  Serial1.print("A5 = ");
  Serial1.println(a5);
  /*
    if (a4 < a5){
    motor0.setSpeed(50);
    delay(1000);
    motor0.setSpeed(0);
    }
    else {
    motor1.setSpeed(50);
    delay(1000);
    motor1.setSpeed(0);
    }
    delay(1000);
  */
  digitalWrite(M1_D0, HIGH);
  digitalWrite(M1_D1, LOW);
  analogWrite(3, 200);
  //digitalWrite(M1_EN, HIGH);
  //sbi(TCCR4A, COM4B1);
  //TCCR4A = COM4B1 | PWM4B;
  //OCR4B = 127; // set pwm duty


  delay(500);
  digitalWrite(M1_EN, LOW);
}