/*! \file servo.c \brief Interrupt-driven RC Servo function library. */
//*****************************************************************************
//
// File Name   : 'servo.c'
// Title      : Interrupt-driven RC Servo function library
// Author      : Pascal Stang - Copyright (C) 2002
// Modified      : Jesse Welling - Copyright (C) 2008
// Created      : 7/31/2002
// Revised      : 8/02/2008
// Version      : 1.0.1
// Target MCU   : Atmel AVR Series
// Editor Tabs   : 4
//
// This code is distributed under the GNU Public License
//      which can be found at http://www.gnu.org/licenses/gpl.txt
//
//*****************************************************************************

#include <avr/io.h>
#include <avr/interrupt.h>

#include "FreeRTOS.h"
#include "task.h"
#include "semphr.h"

#include "../avrlib/global.h"
#include "../avrlib/uart.h"    // include uart function library
#include "../avrlib/rprintf.h" // include printf function library

#include "servo.h"
#include "../Robostix.h"

/*roll over number*/
#define TOP   ((uint16_t)40000u)

// handy to think with values
#define CENTER_POSITION_MSX100   1500u
#define MIN_POSITION_MSX100      1000u
#define MAX_POSITION_MSX100      2000u

// actual raw values
#define CENTER_POSITION_RAW      (CENTER_POSITION_MSX100 * 2)  // 3000
#define MIN_POSITION_RAW         (MIN_POSITION_MSX100 * 2) // 2000
#define MAX_POSITION_RAW         (MAX_POSITION_MSX100 * 2) // 4000

/* Note: Since only 500 millisecondsX100 is between center and min, and likewise center to max, when multiplied
 *       by 2 to get the raw value that gives 1000 which then can be used as a signed percentX10 from center.
 *       Nifty!
 */

// Global variables
// servo channel registers
static volatile uint16_t ServoPosTics;
static volatile uint8_t  ServoChannel;
static const uint16_t ServoPeriodTics = TOP;

xTaskHandle ServoDebugTaskHandle;
xSemaphoreHandle DebugServoSynchro = NULL;

typedef struct
{  
   uint8_t  chan;
   uint16_t inc;
   uint16_t nxt;
   uint16_t ocval;
   uint16_t srvpos;
}TimerDebug_T;

TimerDebug_T ServoTimerDebugBuffer[3];
TimerDebug_T ServoTimerDebugBufferPRINT[3];

// functions

static void DebugServo( void * param);

//! initializes software PWM system
void servoInit(void)
{
   uint8_t channel;
   
   //TODO REMOVE DEBUG
   uartInit();
   uartSetBaudRate(115200);
   rprintfInit(uartSendByte);

   // enable and clear channels
   for(channel=0; channel<NumberOfServoChannels; channel++)
   {
      // set minimum position as default
      ServoChannels[channel]->duty_raw = CENTER_POSITION_RAW;

      // set up DDR for each port.
      *(DDR(ServoChannels[ServoChannel]->port)) |= ServoChannels[ServoChannel]->pin_mask;

      // turn off current channel to break out of tristate. see page 67 of the data sheet
      *(ServoChannels[ServoChannel]->port) &= ~(ServoChannels[ServoChannel]->pin_mask);
   }

   // disble the timer3 output compare A interrupt
   cbi(ETIMSK, OCIE3A);

   /* setup timer 3
    * 
    * TCCR3A Bit 7:6 – COMnA1:0: Compare Output Mode for Channel A
    * TCCR3A Bit 5:4 – COMnB1:0: Compare Output Mode for Channel B
    * TCCR3A Bit 3:2 – COMnC1:0: Compare Output Mode for Channel C
    * TCCR3A Bit 1:0 – WGMn1:0: Waveform Generation Mode
    *
    * TCCR3B Bit 4:3 – WGMn3:2: Waveform Generation Mode
    * TCCR3B Bit 2:0 – CSn2:0: Clock Select
    *
    * TCCR3C Bit 7 – FOCnA: Force Output Compare for Channel A
    * TCCR3C Bit 6 – FOCnB: Force Output Compare for Channel B
    * TCCR3C Bit 5 – FOCnC: Force Output Compare for Channel C
    *
    */

   TCCR3A |= 0x00; 
   TCCR3B = 0x02;
   TCCR3C |= 0x00; 

   // set PosTics
   ServoPosTics = 0;

   // set initial interrupt time
   uint16_t OCValue = 0;
   
   ServoChannel = NumberOfServoChannels + 1; 

   // read in current value of output compare register OCR3A
   //OCValue = OCR3A;      // read OCR3A
   OCValue =  inb(OCR3AL);      // read low byte of OCR3A
   OCValue += inb(OCR3AH)<<8;   // read high byte of OCR3A

   // increment OCR3A value by nextTics
   OCValue += ServoPeriodTics; 

   // set future output compare time to this new value
   //OCR3A = OCValue;         // write 16bit 
   outb(OCR3AH, (OCValue>>8));         // write high byte
   outb(OCR3AL, (OCValue & 0x00FF));   // write low byte

   // enable the timer3 output compare A interrupt
   sbi(ETIMSK, OCIE3A);
}

//! turns off software PWM system
void servoOff(void)
{
   // disable the timer3 output compare A interrupt
   cbi(ETIMSK, OCIE3A);
}

//! set servo position on channel
void servoSetPosition(ServoChannel_T* this, int16_t percentX10)
{
   // input should be between 0 and SERVO_POSITION_MAX
   uint16_t pos_scaled;

   percentX10 = MAX(percentX10, -1000);
   percentX10 = MIN(percentX10, 1000);
   
   // calculate scaled position
   pos_scaled = (uint16_t)( percentX10 + CENTER_POSITION_RAW);

   // set position
   servoSetPositionRaw(this, pos_scaled);
}

//! get servo position on channel
int16_t servoGetPosition(ServoChannel_T* this)
{
   return ( (int16_t)servoGetPositionRaw(this) - CENTER_POSITION_RAW );
}

//! set servo position on channel (raw unscaled format)
void servoSetPositionRaw(ServoChannel_T* this, uint16_t position)
{
   // bind to limits
   position = MAX(position, MIN_POSITION_RAW);
   position = MIN(position, MAX_POSITION_RAW);

   // set position
   this->duty_raw = position;

   //TODO REMOVE DEBUG
    
   /*
   rprintfu08(this->pin_mask);
   rprintfChar('@');
   rprintfu08(this->port);
   rprintfChar(':');
   rprintfu16(this->duty_raw);
   rprintfCRLF();
   */
   
}

//! get servo position on channel (raw unscaled format)
uint16_t servoGetPositionRaw(ServoChannel_T* this)
{
   return this->duty_raw;
}

void PrintServoDebug(void)
{
   ServoTimerDebugBufferPRINT[0] = ServoTimerDebugBuffer[0];
   ServoTimerDebugBufferPRINT[1] = ServoTimerDebugBuffer[1];
   ServoTimerDebugBufferPRINT[2] = ServoTimerDebugBuffer[2];

   {
      rprintfu08(ServoTimerDebugBufferPRINT[0].chan);
      rprintfChar(' ');
      rprintfu16(ServoTimerDebugBufferPRINT[0].inc);
      rprintfChar(' ');
      rprintfu16(ServoTimerDebugBufferPRINT[0].ocval);
      rprintfChar(' ');
      rprintfu16(ServoTimerDebugBufferPRINT[0].nxt);
      rprintfChar(' ');
      rprintfu16(ServoTimerDebugBufferPRINT[0].srvpos);
      rprintfCRLF();
   }

   {
      rprintfu08(ServoTimerDebugBufferPRINT[1].chan);
      rprintfChar(' ');
      rprintfu16(ServoTimerDebugBufferPRINT[1].inc);
      rprintfChar(' ');
      rprintfu16(ServoTimerDebugBufferPRINT[1].ocval);
      rprintfChar(' ');
      rprintfu16(ServoTimerDebugBufferPRINT[1].nxt);
      rprintfChar(' ');
      rprintfu16(ServoTimerDebugBufferPRINT[1].srvpos);
      rprintfChar(' ');
      rprintfu08(*DDR(ServoChannels[0]->port));
      rprintfChar(' ');
      rprintfu08(*(ServoChannels[0]->port)) ;
      rprintfCRLF();
   }

   {
      rprintfu08(ServoTimerDebugBufferPRINT[2].chan);
      rprintfChar(' ');
      rprintfu16(ServoTimerDebugBufferPRINT[2].inc);
      rprintfChar(' ');
      rprintfu16(ServoTimerDebugBufferPRINT[2].ocval);
      rprintfChar(' ');
      rprintfu16(ServoTimerDebugBufferPRINT[2].nxt);
      rprintfChar(' ');
      rprintfu16(ServoTimerDebugBufferPRINT[2].srvpos);
      rprintfChar(' ');
      rprintfu08(DDRB);
      rprintfChar(' ');
      rprintfu08(*DDR(&PORTB));
      rprintfCRLF();
   }
   rprintfCRLF();
}

void StartDebugServoTask(void)
{
   vSemaphoreCreateBinary( DebugServoSynchro );
   xTaskCreate (DebugServo, "DbSrv", configMINIMAL_STACK_SIZE, NULL, ( tskIDLE_PRIORITY + 2), &ServoDebugTaskHandle);
}

static void DebugServo( void * param)
{
   for(;;)
   {
      if( xSemaphoreTake( DebugServoSynchro , 0xFFFF ) == pdTRUE )
      {
         PrintServoDebug();
         LED_TOGGLE( YELLOW );
      }
   }
}

ISR(TIMER3_COMPA_vect)
{
   uint16_t nextTics;
   uint16_t OCValue;
   portBASE_TYPE TaskWoken = pdFALSE; 

   if(ServoChannel < NumberOfServoChannels)
   {
      // turn off current channel
      *(ServoChannels[ServoChannel]->port) &= ~(ServoChannels[ServoChannel]->pin_mask);
   }
   
   // next channel
   ServoChannel++;

   if(ServoChannel != NumberOfServoChannels)
   {
      // loop to channel 0 if needed
      if(ServoChannel > NumberOfServoChannels)   ServoChannel = 0;
      // turn on new channel
      *(ServoChannels[ServoChannel]->port) |= ServoChannels[ServoChannel]->pin_mask;
      // schedule turn off time
      nextTics = ServoChannels[ServoChannel]->duty_raw;
   }
   else //(Channel == NumberOfServoChannels)
   {
      // ***we could save time by precalculating this
      // schedule end-of-period
      nextTics = ServoPeriodTics - ServoPosTics;

      xSemaphoreGiveFromISR(DebugServoSynchro , &TaskWoken);
      if (TaskWoken == pdTRUE)
      {
         taskYIELD();
      }
   }

   // read in current value of output compare register OCR3A
   // OCValue = OCR3A;   // read all 16 bits of OCR3A
   OCValue =  inb(OCR3AL);      // read low byte of OCR3A
   OCValue += inb(OCR3AH)<<8;   // read high byte of OCR3A

   //TODO REMOVE DEBUG
   ServoTimerDebugBuffer[ServoChannel].chan = ServoChannel;
   ServoTimerDebugBuffer[ServoChannel].ocval = OCValue;
   ServoTimerDebugBuffer[ServoChannel].inc++;

   // increment OCR3A value by nextTics
   OCValue += nextTics;

   //   OCR3A+=nextTics; // but must protect the register so byte wise shift it in
   // set future output compare time to this new value
   // OCR3A = OCValue;
   outb(OCR3AH, (OCValue>>8));         // write high byte
   outb(OCR3AL, (OCValue & 0x00FF));   // write low byte

   // set our new tic position
   ServoPosTics += nextTics;
   if(ServoPosTics >= ServoPeriodTics)   ServoPosTics = 0;

   //TODO REMOVE DEBUG
   ServoTimerDebugBuffer[ServoChannel].nxt = nextTics;
   ServoTimerDebugBuffer[ServoChannel].srvpos = ServoPosTics;
}