/*
  pwm_control.c - pwm control methods
  Part of Grbl v0.9

  Copyright (c) 2012-2014 Sungeun K. Jeon

  Grbl is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  Grbl is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
*/
/* 
  This file is based on work from Grbl v0.8, distributed under the 
  terms of the MIT-license. See COPYING for more details.  
    Copyright (c) 2009-2011 Simen Svale Skogsrud
    Copyright (c) 2012 Sungeun K. Jeon
*/ 

#include "system.h"
#include "pwm_control.h"
#include "protocol.h"
#include "print.h"
#include "gcode.h"
#include "settings.h"


uint8_t prescaler=0; 
static inline void pwm_on()  { TCCR5B|=prescaler; PORTB|=0x80; }
static inline void pwm_off() { TCCR5B&=~7; PORTB&=~0x80; }
static inline char pwm_tst() { return TCCR5B&7; }


void pwm_stop()
{
  pwm_off();
}

void WDT_off(void)
{
	cli();
	wdt_reset();
	/* Clear WDRF in MCUSR */
	MCUSR &= ~(1<<WDRF);
	/* Write logical one to WDCE and WDE */
	/* Keep old prescaler setting to prevent unintentional time-out
	*/
	WDTCSR |= (1<<WDCE) | (1<<WDE);
	/* Turn off WDT */
	WDTCSR = 0x00;
	sei();
}

void
pwm_init() {
	uint32_t top=settings.pwm_step_uS; // set prescaler/interval
	if(!(settings.gcode_flag&16)) return;
	TCCR5B&=~7; // stop counter
	prescaler=1; 
	top*=15;	// CPU_freq = uS
	     if(top>0xffff) { prescaler++; top>>=3;	//   8
	     if(top>0xffff) { prescaler++; top>>=3;	//   64
	     if(top>0xffff) { prescaler++; top>>=2;	//  256
	     if(top>0xffff) { prescaler++; top>>=2;	// 1024	
	     if(top>0xffff) { top=0xffff;
	}}}}}
	atomic(TCNT5=top>>2);
	atomic(ICR5=top>>1);
#if	  PWM_STEP == 3
	TCCR5B=bit(WGM53)|bit(WGM52)|bit(COM5A0); // CTC mode, ICR , toggle B
	atomic(OCR4A=top);	// 50% pwm
#else
#if	  PWM_STEP == 4
	TCCR5B=bit(WGM53)|bit(WGM52)|bit(COM5B0); // CTC mode, ICR , toggle B
	atomic(OCR4B=top>>1);	// 50% pwm
#else
#if	  PWM_STEP == 5
	TCCR5B=bit(WGM53)|bit(WGM52)|bit(COM5C0); // CTC mode, ICR , toggle B
	atomic(OCR4C=top>>1);	// 50% pwm
#else
#error	"wrong definition of pwm-step"
#endif
#endif
#endif
 	//TIMSK5=bit(TOIE5);
	PWM_STEP_DDR|=1<<PWM_STEP;
	PWM_STEP_PORT&=~bit(PWM_STEP);
	//TCCR5B|=bit(WGM53)|prescaler ; // 1-5 1 - 8 - 64 - 256 - 1024
	TCCR5B|=bit(WGM53); // 1-5 1 - 8 - 64 - 256 - 1024
	TCCR5B|=bit(WGM53)|prescaler; // 1-5 1 - 8 - 64 - 256 - 1024
}


void
pwm_run() {
  uint8_t t=3;t+=settings.pwm_time&7;
	if(!(settings.gcode_flag&16)) return;
	if(!settings.pwm_step_uS) return;
	if(t==10) t--;	// illegal value
  
  if (sys.state == STATE_CHECK_MODE) { return; }

  protocol_auto_cycle_start();  //temp fix for M3 lockup
  protocol_buffer_synchronize(); 

  
	WDT_off();
  cli();
  if(!(WDTCSR&7)) { 
	MCUSR &= ~(1<<WDRF);
	WDTCSR  = (1<<WDCE) | (1<<WDE);
	/* Set new prescaler(time-out) value = 64K cycles (~0.5 s) */
	WDTCSR  = (1<<WDP0) |(1<<WDP1) |(1<<WDP2) | (1<<WDE);
    	pwm_on();
	}
   sei();
    
}

  ISR(WDT_vect) // Watchdog timer ISR
  {
    pwm_off();
    TCNT5=0;
    WDT_off();
  }