const int analogInPin = A0; // Analog input pin ADC0 (PB5)

#include "TinyDebug.h"
#include <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>

#define LED_PIN PB2
#define SERVO_PIN PB1

// Trim Duration is about the total combined time spent inside the Compare Match ISR
// This time is in timer ticks, where each tick is always 8 microseconds.
#define TRIM_DURATION 4

#define USE_TIMER0

//=============================================================================
// Servo Sequencer types
//=============================================================================
//This enum defines the states of the Servo Sequencer
enum SequencerState_t
{
  WAITING_FOR_512_MARK,
  WAITING_TO_SET_PIN_LOW,
  WAITING_FOR_2048_MARK,
  WAITING_TO_SET_PIN_HIGH
};

//This struct defines a single servo slot
struct ServoEntry
{
    uint8_t  pulseLengthInTicks;    //length of pulse in ticks after offset is applied
    uint8_t  pin;                   //which pin to pulse on portB
    bool     enabled;               //True when this servo should be pulsed
    bool     slotOccupied;          //True when this servo entry is allocated to a servo
};

static const    uint16_t          kMaxNumberOfServosSupported = 5;  //The number of servos to support. See NOTE1 below.
static volatile SequencerState_t  state;                            //The current state of the driver
static          ServoEntry        servoRegistry[kMaxNumberOfServosSupported]; //The array of servo slots
static volatile uint8_t           servoIndex;                       //The index of the current servo slot we are working with.



#ifdef USE_TIMER0
  #define TCNTn   TCNT0
  #define OCRnx   OCR0A
  #define OCFnx   OCF0A
  #define OCIEnx  OCIE0A
#endif

#ifdef USE_TIMER1
  #define TCNTn   TCNT1
  #define OCRnx   OCR1A
  #define OCFnx   OCF1A
  #define OCIEnx  OCIE1A
#endif

//=============================================================================
// FUNCTION:    void setupTimerPrescaler()
//
// DESCRIPTION: Helper function that sets up the timer prescaller based on what
//              timer is selected and the F_CPU frequence.
//
// INPUT:       Nothing
//
// RETURNS:     Nothing
//
//=============================================================================
void setupTimerPrescaler()
{
    #ifdef USE_TIMER0
        //reset the Timer Counter Control Register to its reset value
        TCCR0B = 0;

        #if F_CPU == 8000000L
            //set counter0 prescaler to 64
            //our FCLK is 8mhz so this makes each timer tick be 8 microseconds long
            TCCR0B &= ~(1<< CS02); //clear
            TCCR0B |=  (1<< CS01); //set
            TCCR0B |=  (1<< CS00); //set

        #elif F_CPU == 1000000L
            //set counter0 prescaler to 8
            //our F_CPU is 1mhz so this makes each timer tick be 8 microseconds long
            TCCR0B &= ~(1<< CS02); //clear
            TCCR0B |=  (1<< CS01); //set
            TCCR0B &= ~(1<< CS00); //clear
        #else
            //unsupported clock speed
        #error Unsupported clock speed. This code will only operate at 8Mhz or 1Mhz
        #endif
    #endif


    #ifdef USE_TIMER1
        //reset the Timer Counter Control Register to its reset value
        TCCR1 = 0;

        #if F_CPU == 8000000L
            //set counter1 prescaler to 64
            //our F_CPU is 8mhz so this makes each timer tick be 8 microseconds long
            TCCR1 &= ~(1<< CS13); //clear
            TCCR1 |=  (1<< CS12); //set
            TCCR1 |=  (1<< CS11); //set
            TCCR1 |=  (1<< CS10); //set

        #elif F_CPU == 1000000L
            //set counter1 prescaler to 8
            //our F_CPU is 1mhz so this makes each timer tick be 8 microseconds long
            TCCR1 &= ~(1<< CS13); //clear
            TCCR1 |=  (1<< CS12); //set
            TCCR1 &= ~(1<< CS11); //clear
            TCCR1 &= ~(1<< CS10); //clear
        #else
            //unsupported clock speed
        #error Unsupported clock speed. This code will only operate at 8Mhz or 1Mhz
        #endif
    #endif
}//end setupTimerPrescaler


void servoTimerSetup()
{
    //set up the timer prescaler based on which timer was selected and our F_CPU clock
    setupTimerPrescaler();

    // Enable Output Compare Match Interrupt
    TIMSK |= (1 << OCIEnx);

    //reset the counter to 0
    TCNTn  = 0;
    //set the compare value to any number larger than 0
    OCRnx = 255;
    // Enable global interrupts
    sei();

    /*
    TCNT0 - The Timer/Counter
    OCR0A and OCR0B - Output Compare Registers
    TIFR0 - Timer Interrupt Flag Register
    TIMSK - Timer Interrupt Mask Register
    TCCR0B Timer/Counter Control Register B
    */

}//end servoTimerSetup


//=============================================================================
// FUNCTION:    void timerCompareMatchISR()
//
// DESCRIPTION: Interrupt service routine for timer0 compare A match.
//              This is where the magic happens.
//
// INPUT:       Nothing
//
// RETURNS:     Nothing
//=============================================================================
void timerCompareMatchISR()
{
  PORTB = (PINB + 1) && 0x0f;
  /*
  switch (state)
  {
  case WAITING_TO_SET_PIN_HIGH:
    //go to the next servo in the registry
    //if we are the end of the registry, go to the beginning of it
    if(servoIndex == kMaxNumberOfServosSupported - 1)
    {
      servoIndex = 0;
    }
    else
    {
      ++servoIndex;
      //we are not at the end, leave the servo index as is
    }

    //if this servo is enabled set the pin high
    if( servoRegistry[servoIndex].enabled == true )
    {
        PORTB |= (1 << servoRegistry[servoIndex].pin);
    }
    else
    {
        //This servo position is not enabled, don't manipulate the pin
    }

    //reset the counter to 0
    TCNTn  = 0;
    //set the compare value to 64 (512 us). This is the constant pulse offset.
    OCRnx = 64 - TRIM_DURATION; //trim off 4 ticks (32us), this is about the total combined time we spent inside this ISR;
    //update our state
    state = WAITING_FOR_512_MARK;
    break;


  case WAITING_FOR_512_MARK:
    //set the compare value to the additional amount of timer ticks the pulse should last
    OCRnx = servoRegistry[servoIndex].pulseLengthInTicks;
    //update our state
    state = WAITING_TO_SET_PIN_LOW;

    //reset the counter to 0
    TCNTn  = 0;

    //Did we just set OCRnx to zero?
    if(OCRnx == 0)
    {
        //Since we are setting OCRnx and TCNTn to 0 we are not going to get an interrupt
        //until the counter overflows and goes back to 0.
        //set the counter its highest value, to have it overflow right away.
        TCNTn = 0xFF;
        //This will cause this interrupt to fire again almost immediately (at the next timer tick)
    }
    else
    {
        //otherwise we need to clear the OCF0A flag because it is possible that the
        //counter value incremented and matched the output compare value while this
        //function was being executed
        TIFR = (1 << OCF0A);  // write logical 1 to the OCF0A flag to clear it
                              // also have to write 0 to all other bits for this to work.
    }
    break;


  case WAITING_TO_SET_PIN_LOW:
    //if this servo is enabled set the pin low
    if( servoRegistry[servoIndex].enabled == true )
    {
        PORTB &= ~(1 << servoRegistry[servoIndex].pin);
    }
    else
    {
        //This servo position is not enabled, don't manipulate the pin
    }

    //check if the length of this pulse is 2048 microseconds or longer
    if( (64 + servoRegistry[servoIndex].pulseLengthInTicks) > 255 )
    {
        //This pulse length has passed the 2048 us mark, so we skip state WAITING_FOR_2048_MARK
        //update state
        state = WAITING_TO_SET_PIN_HIGH;
        //set the compare value to the amount of time (in timer ticks) we need to wait to reach
        //4096 microseconds mark
        //which is 512 minus the total pulse length. (resulting number will be between 0 and 255 inclusive)
        OCRnx = 512 - (64 + servoRegistry[servoIndex].pulseLengthInTicks);
    }
    else
    {
        //This pulse length has not reached the 2048 us mark, therefor we have to get to that mark first
        //update state
        state = WAITING_FOR_2048_MARK;
        //set OCRnx to the amount of time (in timer ticks) we have to wait to reach this mark
        //which is 255 minus the total pulse length
        OCRnx = 255 - (64 + servoRegistry[servoIndex].pulseLengthInTicks);
    }

    //reset the counter to 0
    TCNTn  = 0;

    break;

  case WAITING_FOR_2048_MARK:
    //update state
    state = WAITING_TO_SET_PIN_HIGH;
    //reset the counter to 0
    TCNTn  = 0;
    //set the compare value to the longest length of time, 255 ticks, or 2040 microseconds
    //This will take us to the ~4096 microsecond mark,
    //at which point the cycle starts again with the next servo slot.
    OCRnx = 255;
    break;
  }//end switch
  */
}//end timerCompareMatchISR


//=============================================================================
// Non Memeber Functions
//=============================================================================

//only define this ISR if we are using TIMER0
#ifdef USE_TIMER0
  //=============================================================================
  // FUNCTION:    Interrupt service routine for timer0 compare A match
  //
  // DESCRIPTION: AVR Libc provided function that is vectored into when the
  //              timer0 compare A match interrupt fires.
  //
  // INPUT:       Nothing
  //
  // RETURNS:     Nothing
  //=============================================================================
  ISR(TIM0_COMPA_vect)
  {
      timerCompareMatchISR();
  }//end ISR TIM0_COMPA_vect
#endif



//only define this ISR if we are using TIMER1
#ifdef USE_TIMER1
  //=============================================================================
  // FUNCTION:    Interrupt service routine for timer1 compare A match
  //
  // DESCRIPTION: AVR Libc provided function that is vectored into when the
  //              timer0 compare A match interrupt fires.
  //
  // INPUT:       Nothing
  //
  // RETURNS:     Nothing
  //=============================================================================
  ISR(TIM1_COMPA_vect)
  {
      timerCompareMatchISR();
  }//end ISR TIM0_COMPA_vect
#endif



void setup()
{
  pinMode(LED_PIN, OUTPUT);
  DDRB = 0x0f;
  Debug.begin();
  Debug.println(F("Hello Tiny!"));
  Debug.print(F("SERVO_PIN:"));
  Debug.println(SERVO_PIN);
  // Debug.print(F("myServo.attached: "));
  // Debug.println(myServo.attached());
  // Debug.print(F("myServo.attach: "));
  // Debug.println(myServo.attach(SERVO_PIN));
  // Debug.print(F("myServo.attached: "));
  // Debug.println(myServo.attached());
  // myServo.detach();
  // Debug.print(F("myServo.attached: "));
  // Debug.println(myServo.attached());


  // Debug.print(F("myServo.write(0): "));
  // Debug.println(myServo.write(0));   //rotate to the 0 degree position
  // Debug.print(F("myServo.readMicroseconds(): "));
  // Debug.println(myServo.readMicroseconds());
  // delay(500);        //wait 2 seconds

  // Debug.print(F("myServo.write(180): "));
  // Debug.println(myServo.write(180));   //rotate to the 180 degree position
  // Debug.print(F("myServo.readMicroseconds(): "));
  // Debug.println(myServo.readMicroseconds());
  // delay(500);        //wait 2 seconds

  // Debug.print(F("myServo.write(90): "));
  // Debug.println(myServo.write(90));   //rotate to the 90 degree position
  // Debug.print(F("myServo.readMicroseconds(): "));
  // Debug.println(myServo.readMicroseconds());
  // delay(500);        //wait 2 seconds
  servoTimerSetup();
}


//sweep the servo
void loop()
{
/*  digitalWrite(LED_PIN, HIGH);
  // for (int pos = 0; pos < 180; pos++) // goes from 0 degrees to 180 degrees
  { // in steps of 1 degree
    // Read the analog input value
    int sensorValue = analogRead(analogInPin);

    // Convert the analog reading (which goes from 0 - 1023) to a voltage (0 - 5V)
    int pos = sensorValue * 180.0 / 1023.0;

    // Print the results to the Debug monitor
    Debug.print("Sensor Value: ");
    Debug.print(sensorValue);
    Debug.print(" | pos: ");
    Debug.print(pos);
    Debug.println(" V");

    myServo.write(pos);             // tell servo to go to position in variable 'pos'
    for (volatile uint16_t i = 0; i < 0xffff; i++);
  }

  digitalWrite(LED_PIN, LOW);
  // for (int pos = 180; pos > 1; pos--) // goes from 180 degrees to 0 degrees
  // {
  //   myServo.write(pos);             // tell servo to go to position in variable 'pos'
  for (volatile uint16_t i = 0; i < 0xffff; i++);
  // }*/
}