/**
 * Firmware for PPM Switch based on AtMega88
 * Achim Walther, www.voidpointer.de
 * version: 1.02, 03.01.2010
 * change: fixed bug in status transition from AUTO to NORC
 * todo: implement optional "crypto" version to detect and avoid hijacking
 */

#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/eeprom.h> 
#include <util/twi.h>

#ifndef F_CPU
#define F_CPU 8000000UL     // 8 Mhz
#endif

#define MIN_SYNC_LEN 4000L
#define MIN_PULSE_LEN 800L
#define MAX_PULSE_LEN 2200L
#define NEUTRAL_PULSE_LEN 1500L
#define NEG_PULSE_LEN 200L
#define NORM_FRAME_LEN 22500L
#define SYNC_SPACER   NORM_FRAME_LEN / 5
#define MIN_FRAME_LEN NORM_FRAME_LEN - SYNC_SPACER
#define MAX_FRAME_LEN NORM_FRAME_LEN + SYNC_SPACER

#define MIN_NUM_VALID_FRAMES 2      // number of valid frames required to switch back from NORC or FAIL1 status
#define MAX_NUM_INVALID_FRAMES 4    // number of invalid frames required to switch to NORC or FAIL1 status

/**
 * The number of channels the R/C is transmitting.
 * To be valid, each PPM frame must exactly match this number of pulses, no more or less.
 * This constant is also used to declare the size of several arrays.
 * When switching to a different PPM transmitter, change this value!
 */
#define NUM_PULSE_PER_FRAME 7

#define PPM_EDGE_POSITIVE 0     // 0 for negative PPM edges
#define SWITCH_CHANNEL 7        // the PPM channel to activate the auto pilot, count starts from 1, value must be NUM_PULSE_PER_FRAME or lower

#define SOFTSTEP 8          // pulse width in microseconds for stepping towards the target position in soft drive mode

#define MAX_TWI_MSG_LEN 21
#define OWN_ADDR 0b11000000 // 0xC0 = this devices I2C slave address ignoring general calls
				            // set bit 0 to respond to the general call address (0x00)
#define TWI_ACK (1<<TWINT)|(1<<TWEA)|(1<<TWIE)|(1<<TWEN)

#define STATE_FAIL1 0       // no valid PPM and no AP commands -> use neutral values from EEPROM
#define STATE_FAIL2 1       // valid PPM, switch on auto but no AP commands -> manual control
#define STATE_NORC  2       // no valid PPM

#define STATE_MAN   4       // manual control via R/C
#define STATE_AUTO1 5       // switch on auto1
#define STATE_AUTO2 6       // switch on auto2

#define SWITCH_THRESHOLD_LOW 1300L
#define SWITCH_THRESHOLD_HIGH 1700L

#define AP_TIMEOUT 153      // the number of timer overflows without AP commands after that the state switches to FAIL. 1 sec is 15.2587890625

uint16_t inPW[NUM_PULSE_PER_FRAME];         // servo values from R/C receiver
uint16_t neutralPW[NUM_PULSE_PER_FRAME];    // neutral servo values stored in EEPROM

uint16_t autoPW[NUM_PULSE_PER_FRAME];       // servo values set by auto pilot, defaults from EEPROM
uint16_t trgtPW[NUM_PULSE_PER_FRAME];      // the target position in soft drive mode
uint8_t  softDr[NUM_PULSE_PER_FRAME];        // flag for soft drive for each servo

//uint16_t avgFrameLen = NORM_FRAME_LEN;
uint16_t stat[NUM_PULSE_PER_FRAME];         // statistical data like avgFrameLen, totalValidFrames, totalInvalidFrames

volatile uint8_t switchState = STATE_FAIL1;

volatile uint8_t inMsgByteCount = 0;
volatile uint8_t outMsgByteCount = 0;
volatile uint8_t i2cIn[MAX_TWI_MSG_LEN];
volatile uint8_t i2cFinished = 0;

//volatile uint8_t debugRed = 0;
//volatile uint8_t debugGreen = 0;
static inline uint8_t isValidPulseLen(const uint16_t pluseLen)
{
    return (pluseLen > MIN_PULSE_LEN && pluseLen < MAX_PULSE_LEN);
}

void writeNeutralValues()
{
    uint8_t i;
    // get EEPROM access counter from address 1
    uint16_t eeCounter = eeprom_read_word((uint16_t *) 1);  // leave out eeprom address 0
    // increase access counter
    eeCounter++;
    // write Counter
    eeprom_write_word((uint16_t *) 1, eeCounter);
    // write array length at address 3
    eeprom_write_byte((uint8_t *) 3, NUM_PULSE_PER_FRAME);
    // write all servo neutral values from i2cIn array into EEPROM
    for (i=0; i<NUM_PULSE_PER_FRAME; i++) {
        eeprom_write_word((uint16_t *)(2*i+4), neutralPW[i]);
    }
}

void initNeutralValues()
{
    // check if neutral values are stored in EEPROM
    uint8_t i;
    uint8_t count = eeprom_read_byte((uint8_t *) 3);  // array length at address 3
    if (count & 0xF0) count = 0;    // if count >= 16 then count is invalid 
    for (i=0; i<NUM_PULSE_PER_FRAME; i++) {
        // use EEPROM value if available, otherwise init with default value (1500 us)
        uint16_t eeVal = (i < count) ? eeprom_read_word((uint16_t *)(2*i+4)) : NEUTRAL_PULSE_LEN;
        neutralPW[i] = eeVal;
        // init AP values as well. Even if timeOutAp works some values may not have been set by the AP
        autoPW[i] = eeVal;
    }

    if (count < NUM_PULSE_PER_FRAME) {
        // not all values were read from EEPROM --> now write all values back
        writeNeutralValues();
    }
}
 
int main(void)
{
    uint16_t lastIcrTime = 0;
    uint8_t inPulseCount = 0;
    uint16_t inSyncTime = 0;
    uint16_t inFrameLen = 0;

    uint8_t validSync = 0;

    uint8_t outPulseCount = 0;
    uint16_t outStartFrame;

    uint8_t invalidFrameCount = 0;
    uint8_t validFrameCount = 0;
    
    uint8_t timerOvrCount = 0;
    uint8_t ppmOvrCount = 0;
    uint8_t apOvrCount = 0;
    
    // init
    initNeutralValues();

    // Power and noise reduction
    PRR = (1<<PRTIM2)|(1<<PRTIM0)|(1<<PRSPI)|(1<<PRUSART0)|(1<<PRADC);

    // set command byte to default 0x80 (read R/C values)
    i2cIn[0] = 0x80;
    
	DDRC = 0b00000011;	// Pin C0 and C1 are LED outputs, C0 is green, C1 red
    DDRB = 0b00000010;  // Input on Pin B0 (ICP1), output on Pin B1 (OC1A)

    OCR1A = NORM_FRAME_LEN;	// Output Timer init with a complete Frame length
    ICR1 = 0;	            // Input Timer init
    
    outStartFrame = OCR1A;

    PORTB = (PPM_EDGE_POSITIVE<<PB1);       // generate init reset frame according to pos / neg PPM mode

    TCCR1A = (1<<COM1A0);   // Toggle OC1A on Compare Match
                            // Free Running Timer
    TCCR1B = (PPM_EDGE_POSITIVE << ICES1)|(1<<CS11);     // prescaler 8, input capture according to PPM edge

    // TWI init
    TWAR = OWN_ADDR;
	// Enable TWI-interface and release TWI pins, enable Interupt, Enable acknowledgement
    TWCR = (1<<TWEN)|(1<<TWIE)|(0<<TWINT)|(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)|(0<<TWWC);
    
    // statistics data
    uint16_t *avgFrameLen, *totalValidFrames, *totalInvalidFrames;
    avgFrameLen = stat;    // the first var points to the start of the stat array
    totalValidFrames = avgFrameLen + 1;
    totalInvalidFrames = avgFrameLen + 2;
    *avgFrameLen = NORM_FRAME_LEN;  // init the average with the default
    
    sei();

    // Main Loop
   	while (1) {
        // Input Capture handling
        if (TIFR1 & (1<<ICF1)) {
            uint16_t currIcrTime = ICR1;  // copy ICR1 value to a local var to avoid false calculations from ICR1 overwrite
            TIFR1 = (1<<ICF1);            // reset ICF1
            uint16_t inPulseLen = currIcrTime - lastIcrTime;
            lastIcrTime = currIcrTime;
            if ((inPulseLen > MIN_SYNC_LEN) && (inPulseLen <= NORM_FRAME_LEN)) {
                // sync detected
                inFrameLen = currIcrTime - inSyncTime;
                inSyncTime = currIcrTime;
                ppmOvrCount = 0;    // reset PPM overrun after a valid sync
                if (validSync    // the last frame had a valid sync and inPulseCount <= NUM_PULSE_PER_FRAME
                    && (inFrameLen > MIN_FRAME_LEN) && (inFrameLen <= MAX_FRAME_LEN)
                    && (inPulseCount == NUM_PULSE_PER_FRAME)) // reject frames with inPulseCount less than NUM_PULSE_PER_FRAME
                {
                    invalidFrameCount = 0;
                    if (validFrameCount < MIN_NUM_VALID_FRAMES) validFrameCount++;	        // must not overflow
                    // update statistics
                    (*totalValidFrames)++;    // counter overflow depending on frame length. For 22ms it is 24 minutes at the earliest
                    // update average frame length
                    *avgFrameLen = ((uint32_t)(*avgFrameLen)*31 + inFrameLen) / 32;
                } else {
                    // the whole last frame was not considered valid
                    validFrameCount = 0;
                    if (invalidFrameCount < MAX_NUM_INVALID_FRAMES) invalidFrameCount++;    // must not overflow
                    // update statistics
                    (*totalInvalidFrames)++;  // counter can overflow, depends on quality of R/C reception
                }
                // start a new frame validation
                validSync = 1;
                inPulseCount = 0;
            } else if (validSync && isValidPulseLen(inPulseLen) && (inPulseCount < NUM_PULSE_PER_FRAME)) {
                inPW[inPulseCount++] = inPulseLen;
            } else {
                // invalid pulsewidth or too many pulses --> discard all gathered info
                validSync = 0;
            }
        }
    
        // consider validFrameCount and invalidFrameCount for determination of R/C reception quality
        uint8_t sufficientPPM = 0;
        if (switchState > STATE_NORC) {
            // currently in a state with sufficient R/C reception
            sufficientPPM = (invalidFrameCount < MAX_NUM_INVALID_FRAMES);
        } else {
            // no reception or reception quality not sufficient
            sufficientPPM = (validFrameCount >= MIN_NUM_VALID_FRAMES);
        }
        
        // status display on every Timer overrun
        if (TIFR1 & (1<<TOV1)) {
            TIFR1 = (1<<TOV1);          // reset TOV1

            if (!(timerOvrCount & 0x03)) {
                if (switchState >= STATE_MAN && timerOvrCount <= (switchState - STATE_MAN) << 2) {
                    // green LED
                    PORTC = 0b00000010;     // LED output for testing
                }
                if (switchState <= STATE_NORC && timerOvrCount <= switchState << 2) {
                    // red LED
                    PORTC = 0b00000001;     // LED output for testing
                }
            } else {
                // LEDs off
                PORTC = 0b00000011;
            }
                
            if (++timerOvrCount > 16) timerOvrCount = 0;
            
            // PPM timeout
            // uses a counter to extend pulse width measuring, if ovrCount > 1 then inPulseLen is out of range
            if (ppmOvrCount < 2) ppmOvrCount++;
                
            // Autopilot timeout, disable AP timeout in MAN mode
            if ((apOvrCount < AP_TIMEOUT) && (switchState != STATE_MAN)) apOvrCount++;
            //if (apOvrCount < AP_TIMEOUT) apOvrCount++;
        }

        // LED debugging
        //if (debugRed && debugGreen) PORTC = 0b00000000; 
        //else {
        //    if (debugRed) PORTC = 0b00000001; else PORTC = 0b00000011;
        //    if (debugGreen) PORTC = 0b00000010; else PORTC = 0b00000011;
        //}

        // state transition
        uint8_t tState = STATE_AUTO1;
        if (sufficientPPM && (ppmOvrCount < 2)) {
            if (inPW[SWITCH_CHANNEL-1] < SWITCH_THRESHOLD_LOW) tState = STATE_MAN;
            if (inPW[SWITCH_CHANNEL-1] > SWITCH_THRESHOLD_HIGH) tState = STATE_AUTO2;

            if ((apOvrCount == AP_TIMEOUT) && (tState==STATE_AUTO1 || tState==STATE_AUTO2)) tState = STATE_FAIL2;
        } else {
            //if (tState==STATE_FAIL2) tState = STATE_FAIL1 // redundant
            tState = STATE_NORC;
            if (apOvrCount == AP_TIMEOUT) tState = STATE_FAIL1;
        }
        // write local var to global volatile var
        switchState = tState;

        // PPM synthesis
        if (TIFR1 & (1<<OCF1A)) {
            TIFR1 = (1<<OCF1A);          // reset OCF1A
            if ((PINB & (1<<PINB1)) == (PPM_EDGE_POSITIVE<<PINB1)) {
                // output pulse is on the positive level
                if (outPulseCount < NUM_PULSE_PER_FRAME) {
                    // next pulse width comes from channel outPulseCount
                    // use different pulse width table depending on switch state
                    if (tState == STATE_MAN || tState == STATE_FAIL2) {
                        // uses the values of the current frame capture if available 
                        OCR1A += inPW[outPulseCount] - NEG_PULSE_LEN;
                    } else if (tState == STATE_FAIL1) {
                        OCR1A += neutralPW[outPulseCount] - NEG_PULSE_LEN;
                    } else {
                        if (softDr[outPulseCount]) {
                            // softDrive
                            int16_t pwDiff = trgtPW[outPulseCount] - autoPW[outPulseCount];
                            if (pwDiff > SOFTSTEP) autoPW[outPulseCount] += SOFTSTEP;
                            else if (pwDiff < -SOFTSTEP) autoPW[outPulseCount] -= SOFTSTEP;
                            else autoPW[outPulseCount] = trgtPW[outPulseCount];
                        }
                        OCR1A += autoPW[outPulseCount] - NEG_PULSE_LEN;
                    }
                    outPulseCount++;
                } else {
                    // create the sync pulse
                    
                    // align with input frame sync if available
                    int16_t syncDiff = 0;
                    if (sufficientPPM) {
                        syncDiff = inSyncTime - outStartFrame + MAX_PULSE_LEN;
                        if (syncDiff > SYNC_SPACER) syncDiff = SYNC_SPACER;
                        else if (syncDiff < -SYNC_SPACER) syncDiff = -SYNC_SPACER;
                    }
                    // OCR1A += NORM_FRAME_LEN - (ORC1A - outStartFrame) - NEG_PULSE_LEN + syncDiff;
                    OCR1A = outStartFrame + syncDiff + *avgFrameLen - NEG_PULSE_LEN;
                    outStartFrame = OCR1A;  // contains the end of the sync pulse
                    outPulseCount = 0;
                }
            } else {
                 // negative pulse level --> add the pulse length for opposite part
                 OCR1A += NEG_PULSE_LEN;
            }
        }
        
        if (i2cFinished) {
            // parse received message
            uint8_t i, s;
            uint16_t sval;
            switch (i2cIn[0]) {
                case 0xA0:      // set all servos, byte values are sx xx sx xx ... where s=softdrive and xxx is pulse width in us
                    for (i=1; i<inMsgByteCount; i += 2) {
                        s = i2cIn[i] >> 4;      // soft drive flag
                        sval = ((i2cIn[i] & 0x0F) << 8) + i2cIn[i+1];
                        if (((i>>1) >=0) && ((i>>1) < NUM_PULSE_PER_FRAME) && isValidPulseLen(sval)) {
                            if (s) {
                                trgtPW[i>>1] = sval;   // set target fpr soft drive
                                // improve soft drive in FAIL mode
                                if (switchState <= STATE_FAIL2) autoPW[i>>1] = neutralPW[i>>1];
                            } else autoPW[i>>1] = sval;   // normal mode
                            softDr[i>>1] = s;
                        }
                    }
                    break;
                case 0xAA:      // set servos, byte values are nx xx nx xx ... where n=servo number and xxx is pulse width in us
                    for (i=1; i<inMsgByteCount; i += 2) {
                        s = (i2cIn[i] >> 4) - 1;      // the servo index, counts from 1, make it 0 based
                        sval = ((i2cIn[i] & 0x0F) << 8) + i2cIn[i+1];
                        if ((s >= 0) && (s < NUM_PULSE_PER_FRAME) && isValidPulseLen(sval)) {
                            autoPW[s] = sval;
                            softDr[s] = 0;
                        }
                    }
                    break;
                case 0xBA:      // set servos softdrive, byte values are nx xx nx xx ... where n=servo number and xxx is pulse width in us
                    for (i=1; i<inMsgByteCount; i += 2) {
                        s = (i2cIn[i] >> 4) - 1;      // the servo index, counts from 1, make it 0 based
                        sval = ((i2cIn[i] & 0x0F) << 8) + i2cIn[i+1];
                        if ((s >= 0) && (s < NUM_PULSE_PER_FRAME) && isValidPulseLen(sval)) {
                            trgtPW[s] = sval;
                            softDr[s] = 1;
                            // improve soft drive in FAIL mode
                            if (switchState <= STATE_FAIL2) autoPW[s] = neutralPW[s];
                        }
                    }
                    break;
                case 0xD0:      // set neutral values for all servos, byte sequence is 0x xx 0x xx 0x xx 0x xx 0x xx .. ..
                    for (i=1; i<inMsgByteCount; i += 2) {
                        sval = ((i2cIn[i] & 0x0F) << 8) + i2cIn[i+1];
                        if (((i>>1) >=0) && ((i>>1) < NUM_PULSE_PER_FRAME) && isValidPulseLen(sval)) {
                            neutralPW[i>>1] = sval;
                        }
                    }
                    // write to EEPROM
                    writeNeutralValues();
                    break;
            }
            
            // reset AP timeout mechanism after a servo command was sent
            if (i2cIn[0] >= 0xA0) apOvrCount = 0;
            
            // reset flag
            i2cFinished = 0;
        }

    } // end while
}


// Interrupt Service Routine for TWI (I2C)
ISR(TWI_vect)
{
    uint8_t i;
    //debugRed = 0;
    //debugGreen = 0;
    switch (TWSR) {
        case TW_SR_SLA_ACK:		// 0x60
	        // start receiving a new message
	        inMsgByteCount = 0;
	        TWCR = TWI_ACK;     // send ACK
            break;
        case TW_SR_DATA_ACK:	// 0x80
    	    // received a data byte
            if (inMsgByteCount < MAX_TWI_MSG_LEN)   // avoid buffer overflow
        	    i2cIn[inMsgByteCount++] = TWDR;
    	    TWCR = TWI_ACK;     // send ACK
            break;
        case TW_SR_DATA_NACK:	// 0x88
    	    // in regular conditions this branch is not called
    	    // switch to the not addressed Slave mode
    	    TWCR = TWI_ACK;     // send ACK
            break;
        case TW_SR_STOP:		// 0xA0
	        // set flag to process received message in main thread
	        i2cFinished = 1;
            //debugGreen = 1;
	        TWCR = TWI_ACK;     // send ACK
            break;

        case TW_ST_SLA_ACK:		// 0xA8
    	    // start of slave transmit
    	    outMsgByteCount = 0;
    	    // send status byte first
    	    TWDR = switchState;
    	    TWCR = TWI_ACK; // send ACK
            break;
        case TW_ST_DATA_ACK:    // 0xB8
        case TW_ST_DATA_NACK:	// 0xC0
        //case TW_ST_LAST_DATA:	// 0xC8
    	    // send data depending on the previous command, if any
    	    i = outMsgByteCount++ >> 1;
            // avoid array index out of range
            if (i < NUM_PULSE_PER_FRAME) {
                uint16_t twVal;
        	    switch (i2cIn[0]) {
                    case 0x84:  // send auto values
        	            twVal = autoPW[i];
        	            break;
                    case 0x88:  // send neutral values
            	        twVal = neutralPW[i];
            	        break;
                    case 0x8C:  // send statistics
        	            twVal = stat[i];
        	            break;
        	        default:    // 0x80 or any other value --> send R/C servo values
        	            twVal = inPW[i];
        	    }
                TWDR = ((outMsgByteCount & 0x01) ? (twVal >> 8) : (twVal & 0x00FF));
            } else {
                // out of range --> send dummy values
                TWDR = 0xFF;
            }

       	    TWCR = TWI_ACK;     // send ACK
            break;
        default: 				// bus error
    	    // set switchState to Error / TimeoutAP
    	    // switch to the not addressed Slave mode
    	    TWCR = (0<<TWSTA)|(1<<TWSTO)|(1<<TWINT)|(1<<TWEA)|(1<<TWIE)|(1<<TWEN);
            //debugRed = 1;
    }
}