/**********Example for showing frame id using telemetry**********
 **********Leonardo Ferraro 2014**********
 */

#include <bitset>
#include "spi.h"
#include "gpio.h"
#include "lepton_i2c.h"


#define PACKET_SIZE 164     // 4 bytes header , 160 bytes payload
#define PACKETS_PER_FRAME 63    // Raw 14bit mode , Telemetry enabled ( as footer or header declared later ) , AGC enabled


int main(){

    uint8_t buf [PACKET_SIZE*PACKETS_PER_FRAME];

    if(lepton_connect() == 0 ){
        std::cout<<"CCI connected to lepton"<<std::endl;
    }

    if(enable_telemetry(1) == 0){               //Enable telemetry as footer
        std::cout<<"Enabling Telemetry"<<std::endl;
    }

    usleep(1000);

    if(telemetry_enabled){
        std::cout<<"Telemetry Enabled"<<std::endl;
    }

    gpio_state(8,1);
    usleep(200); // wait > 185ms to resync
    gpio_state(8,0);

    SpiOpenPort();

    while(1){

        int j;

        for(int i=0 ; i<PACKETS_PER_FRAME ; i++ ){

            read(spi_fd, buf+(sizeof(uint8_t)*PACKET_SIZE*i), sizeof(uint8_t)*PACKET_SIZE );

        }

        /**Index of first 8 bit of first 16bit word for telemetry
         **
         ** first 4 word(8bit) are packet id and CRC , so skip them
         ** 40 is the index first 8 bit word for frame id( 20 is the 16bit word equivalent ), datasheet : 8.3 Telemetry modes
         **
         */

        int frame_id_i = 60*(40 + 4) ;

        unsigned int frame_id = buf[frame_id_i]<<0x18 | buf[frame_id_i+1]<<0x10 | buf[frame_id_i+2]<<0x8 | buf[frame_id_i+3] ;

        std::cout<<"Frame n° : "<<frame_id<<std::endl;

    }

    SpiClosePort();

    return 0;

}