#include "swissspadstream.h"

#include <iostream>
#include <fstream>
#include <string>
using namespace std;

const unsigned SwissSPADStream::HIGH_REG = 0xE<<28;

const unsigned SwissSPADStream::CLOCK_ADR = 1;
const unsigned SwissSPADStream::GETCLKSTATUS_CMD = 0<<28;
const unsigned SwissSPADStream::GETCLKFREQ_CMD = 1<<28;
const unsigned SwissSPADStream::SWITCHCLK_REG = 2<<28;
const unsigned SwissSPADStream::PROGCLK_REG = 3<<28;
const unsigned SwissSPADStream::SHIFTCLK_REG = 4<<28;

const unsigned SwissSPADStream::SHUTTER_ADR = 2;
const unsigned SwissSPADStream::SHUTTER_FLAGS_REG = 0<<28;
const unsigned SwissSPADStream::SHUTTER_HOLDOFF_REG = 3<<28;

const unsigned SwissSPADStream::MAIN_ADR = 3;
const unsigned SwissSPADStream::DUMP_CMD = 3;
const unsigned SwissSPADStream::MEMCLR_CMD = 4;
const unsigned SwissSPADStream::CAPSTART_CMD = 5;
const unsigned SwissSPADStream::ROWLIMITS_REG = 0x9<<28;
const unsigned SwissSPADStream::DEFAULT_DELAY = 11;
const unsigned SwissSPADStream::READOUT_PARAM_REG = 8<<28;
const unsigned SwissSPADStream::MAXCOUNT_REG = 4<<28;
const unsigned SwissSPADStream::IMAGE_LENGTH_REG = 5<<28;
const unsigned SwissSPADStream::FLAGS_REG = 3<<28;
const unsigned SwissSPADStream::FRAME_DELAY_REG = 7<<28;
const unsigned SwissSPADStream::INTER_FRAME_DELAY_REG = 6<<28;
const unsigned SwissSPADStream::IMAGE_COUNT_REG = 0xb<<28;
const unsigned SwissSPADStream::DESCRAMBLER_REG = 0xc<<28;

SwissSPADStream::SwissSPADStream() : FX3Stream() {}

void SwissSPADStream::descramblerOn() {
    sendWord( MAIN_ADR, HIGH_REG|0xff7 );
    sendWord( MAIN_ADR, DESCRAMBLER_REG|0x10000 );
}

void SwissSPADStream::descramblerOff() {
    sendWord( MAIN_ADR, HIGH_REG|0xff7 );
    sendWord( MAIN_ADR, DESCRAMBLER_REG|0 );
}

void SwissSPADStream::readMem( unsigned bitwidth ) {
    setBitWidth(bitwidth);
    sendWord( MAIN_ADR, DUMP_CMD );
}

void SwissSPADStream::clrMem() {
    sendWord( MAIN_ADR, MEMCLR_CMD );
}

void SwissSPADStream::setRowLimits( unsigned min, unsigned max ) {
    sendWord( MAIN_ADR, HIGH_REG|(max&0xfff) ); //max row
    sendWord( MAIN_ADR, ROWLIMITS_REG|(min&0xfff) ); //min row
    //descrambler
    sendWord( MAIN_ADR, HIGH_REG|0xffe );
    sendWord( MAIN_ADR, DESCRAMBLER_REG|((max-min)&0xff) );
}

void SwissSPADStream::setMemIncDelay( unsigned delay ) {
    sendWord( MAIN_ADR, HIGH_REG|((delay&0xff)<<4) );
    sendWord( MAIN_ADR, READOUT_PARAM_REG|(0) ); //set delay
}

void SwissSPADStream::setBitWidth( unsigned bitwidth ) {
    if( bitwidth != 16 && bitwidth != 8 && bitwidth != 4 && bitwidth != 2 && bitwidth != 1 ) {
        cerr << "Illegal bitwidth " << bitwidth << " requested. ";
        unsigned next = 1;
        while( next < bitwidth && next < 16 ) next <<= 1;
        cerr << "Set to " << next << "." << endl;
        bitwidth = next;
    }
    sendWord( MAIN_ADR, HIGH_REG|(0xfef) ); //write only bitwidth
    sendWord( MAIN_ADR, FLAGS_REG|(((bitwidth-1)&0xf)<<4) );
    //descrambler
    sendWord( MAIN_ADR, HIGH_REG|0xffd );
    sendWord( MAIN_ADR, DESCRAMBLER_REG|(((bitwidth-1)&0xf)<<8) );
}

void SwissSPADStream::setMaxCount( unsigned count ) {
    sendWord( MAIN_ADR, HIGH_REG|((count>>24)&0xff) );
    sendWord( MAIN_ADR, MAXCOUNT_REG|(count&0xffffff) );
}

void SwissSPADStream::incMem( unsigned count ) {
    setRowLimits();
    setMaxCount( count );
    sendWord( MAIN_ADR, CAPSTART_CMD );
}

void SwissSPADStream::startCap() {
    sendWord( MAIN_ADR, CAPSTART_CMD );
}

void SwissSPADStream::incRows( unsigned count, unsigned low, unsigned high ) {
    setRowLimits( low, high );
    setMaxCount( count );
    sendWord( MAIN_ADR, CAPSTART_CMD );
}

void SwissSPADStream::updateTiming( unsigned config ) {
    sendWord( MAIN_ADR, HIGH_REG|(1<<3)|((config>>12)&0x7) );
    sendWord( MAIN_ADR, READOUT_PARAM_REG|(config&0xfff) );
}

uint32_t SwissSPADStream::getSystemID( bool quiet ) {
    sendWord( 0xf, 0 );
    uint32_t status;
    int size = receive( 1, &status );
    if( size != 1 ) {
        cerr << "Get ID failed" << endl;
        return 0;
    }
    if( !quiet ) {
        cout << "System ID: " << hex << status << dec << endl;
    }
    return status;
}

unsigned int SwissSPADStream::getStatus( bool quiet ) {
    sendWord( CLOCK_ADR, GETCLKSTATUS_CMD );
    uint32_t status;
    int size = receive( 1, &status );
    if( size != 1 ) {
        cerr << "Get Status failed" << endl;
        return 0;
    }
    if( !quiet ) {
        cout << "Clk status: " << hex << status << dec << endl;
    }
    return status;
}

uint32_t SwissSPADStream::incShift( unsigned amount, bool quiet ) {
    sendWord( CLOCK_ADR, SHIFTCLK_REG|0x10000|(amount&0x3ff) );
    sendWord( CLOCK_ADR, GETCLKSTATUS_CMD );
    uint32_t status;
    int size = receive( 1, &status );
    if( size != 1 ) {
        cerr << "Get Status failed" << endl;
        return 0;
    }
    if( !quiet ) {
        cout << "Shift status: " << hex << status << dec << endl;
    }
    return status;
}

uint32_t SwissSPADStream::decShift( unsigned amount, bool quiet ) {
    sendWord( CLOCK_ADR, SHIFTCLK_REG|0x10000|0x400|(amount&0x3ff) );
    sendWord( CLOCK_ADR, GETCLKSTATUS_CMD );
    uint32_t status;
    int size = receive( 1, &status );
    if( size != 1 ) {
        cerr << "Get Status failed" << endl;
        return 0;
    }
    if( !quiet ) {
        cout << "Shift status: " << hex << status << dec << endl;
    }
    return status;
}

void SwissSPADStream::setMultiFrameMode( bool flag ) {
    sendWord( MAIN_ADR, HIGH_REG|(0xffd) ); //write one flag only
    sendWord( MAIN_ADR, FLAGS_REG|(flag?2:0) );
}

void SwissSPADStream::setSoftRate( bool flag )
{
    sendWord( MAIN_ADR, HIGH_REG|(0xfdf) ); //write one flag only
    sendWord( MAIN_ADR, FLAGS_REG|(flag?0x100:0) );
    //descrambler
    sendWord( MAIN_ADR, HIGH_REG|0xffb );
    sendWord( MAIN_ADR, DESCRAMBLER_REG|(flag?0x1000:0) );
}

void SwissSPADStream::setResetBeforeFrame( bool flag ) {
    sendWord( MAIN_ADR, HIGH_REG|(0xffb) ); //write one flag only
    sendWord( MAIN_ADR, FLAGS_REG|(flag?4:0) );
}

void SwissSPADStream::setShiftMode( bool flag ) {
    sendWord( MAIN_ADR, HIGH_REG|(0xff7) ); //write one flag only
    sendWord( MAIN_ADR, FLAGS_REG|(flag?8:0) );
}

void SwissSPADStream::shutterIdleState( unsigned state ) {
    sendWord( SHUTTER_ADR, HIGH_REG|(0xff8) ); //write one flag only
    sendWord( SHUTTER_ADR, SHUTTER_FLAGS_REG|state );
}

void SwissSPADStream::setContShutter( bool flag ) {
    //in main module
    sendWord( MAIN_ADR, HIGH_REG|(0xffe) ); //write one flag only
    sendWord( MAIN_ADR, FLAGS_REG|(flag?1:0) );
    //in gating module
    sendWord( SHUTTER_ADR, HIGH_REG|(0xfef) ); //write one flag only
    sendWord( SHUTTER_ADR, SHUTTER_FLAGS_REG|(flag?0x10:0) );
}

void SwissSPADStream::setClassicShutter( bool flag ) {
    sendWord( SHUTTER_ADR, HIGH_REG|(0xff7) ); //write one flag only
    sendWord( SHUTTER_ADR, SHUTTER_FLAGS_REG|(flag?0x8:0) );
}

void SwissSPADStream::setShutterHoldoff( unsigned value ) {
    sendWord( SHUTTER_ADR, SHUTTER_HOLDOFF_REG|(value&0xffffff) );
}

void SwissSPADStream::setFrameDelay( unsigned value ) {
    sendWord( MAIN_ADR, FRAME_DELAY_REG|(value&0xffffff) );
}

void SwissSPADStream::setInterFrameDelay( unsigned value ) {
    sendWord( MAIN_ADR, INTER_FRAME_DELAY_REG|(value&0xffffff) );
}

void SwissSPADStream::setImageCount( unsigned value ) {
    sendWord( MAIN_ADR, IMAGE_COUNT_REG|(value&0xffffff) );
}

void SwissSPADStream::setImageLength( unsigned value ) {
    sendWord( MAIN_ADR, IMAGE_LENGTH_REG|(value&0xffffff) );
}

void SwissSPADStream::setShift( unsigned value ) {
    sendWord( SHUTTER_ADR, HIGH_REG|(0xfdf) );
    sendWord( SHUTTER_ADR, (0<<28)|((value&0xf)<<5) ); //gating shift
}

void SwissSPADStream::set_classic_shutter( unsigned length, unsigned repetitions,
    unsigned spadoff_on_value, unsigned spadoff_off_value,
    unsigned recharge_on_value, unsigned recharge_off_value,
    unsigned gate_on_value, unsigned gate_off_value ) 
{
    setClassicShutter( true );
    sendWord( SHUTTER_ADR, (1<<28)|(length&0xffffff) ); //trigger length
    sendWord( SHUTTER_ADR, (2<<28)|(repetitions&0xffffff) ); //repetitions

    sendWord( SHUTTER_ADR, HIGH_REG|(spadoff_on_value&0xfffff) );
    sendWord( SHUTTER_ADR, (4<<28)|(spadoff_off_value&0xfffff) ); //spadoff
    sendWord( SHUTTER_ADR, HIGH_REG|(recharge_on_value&0xfffff) );
    sendWord( SHUTTER_ADR, (5<<28)|(recharge_off_value&0xfffff) ); //recharge
    sendWord( SHUTTER_ADR, HIGH_REG|(gate_on_value&0xfffff) );
    sendWord( SHUTTER_ADR, (6<<28)|(gate_off_value&0xfffff) ); //gate
}

void SwissSPADStream::set_laser_shutter( unsigned length, unsigned repetitions,
    unsigned long long spadoff, unsigned long long recharge, unsigned long long gate )
{
    setClassicShutter( false );
    sendWord( SHUTTER_ADR, (1<<28)|(length&0xffffff) ); //trigger length
    sendWord( SHUTTER_ADR, (2<<28)|(repetitions&0xffffff) ); //repetitions

    sendWord( SHUTTER_ADR, HIGH_REG|((spadoff>>28)&0xfffffff) );
    sendWord( SHUTTER_ADR, (7<<28)|(spadoff&0xfffffff) ); //spadoff
    sendWord( SHUTTER_ADR, HIGH_REG|((recharge>>28)&0xfffffff) );
    sendWord( SHUTTER_ADR, (8<<28)|(recharge&0xfffffff) ); //recharge
    sendWord( SHUTTER_ADR, HIGH_REG|((gate>>28)&0xfffffff) );
    sendWord( SHUTTER_ADR, (9<<28)|(gate&0xfffffff) ); //gate
}

int SwissSPADStream::syncLaserClock( bool sync ) {
    uint32_t data[2];
    sendWord( CLOCK_ADR, GETCLKSTATUS_CMD );
    sendWord( CLOCK_ADR, GETCLKFREQ_CMD ); //read frequency estimate
    if( receive( 2, data ) != 2 ) {
        cerr << "Connection error." << endl;
        return -1;
    }
    
    if( sync ) {
        double estFreqMHz = data[1] / 1000.0;
        cerr << "Estimated external clock frequency: " << estFreqMHz << "MHz. Attempting synchronisation." << endl;
        sendWord( CLOCK_ADR, SWITCHCLK_REG|1 ); //Switch clock
    }
    else {
        sendWord( CLOCK_ADR, SWITCHCLK_REG|0 ); //Switch clock
    }
    
    getStatus(false);
    return 0;
}

