#!/usr/bin/python

import sys
import time
import os

from i2c import COpenCoresI2C

class CSI570:

    # Si570 registers (http://www.silabs.com/Support%20Documents/TechnicalDocs/si570.pdf, page 18-19)

    HS_N1_DIV      = 0x7;
    REF_FREQ_37_32 = 0x8;
    REF_FREQ_31_24 = 0x9;
    REF_FREQ_23_16 = 0xA;
    REF_FREQ_15_8  = 0xB;
    REF_FREQ_7_0   = 0xC;
    RST_MEM_CTRL   = 0x87;
    FREEZE_DCO     = 0x89;
    HS_DIV_LOOKUP  = [4, 5, 6, 7, 0, 9, 0, 11]

    def __init__(self, i2c, addr):
        self.i2c = i2c;
        self.addr = addr;

    def wr_reg16(self, addr, value):
        self.i2c.start(self.addr, True);
        self.i2c.write(addr, False);
        tmp = (value >> 8) & 0xFF;
        self.i2c.write(value, False);
        tmp = value & 0xFF;
        self.i2c.write(value, True)

    def wr_reg8(self, addr, value):
        self.i2c.start(self.addr, True);
        self.i2c.write(addr, False);
        self.i2c.write(value, True);

    def rd_reg16(self, addr):
        self.i2c.start(self.addr, True);
        self.i2c.write(addr, False);
        self.i2c.start(self.addr, False);
        tmp_MSB = self.i2c.read(False);
        tmp_LSB = self.i2c.read(True);
        value = (tmp_MSB << 8) | tmp_LSB;
        return value;

    def rd_reg8(self, addr):
        self.i2c.start(self.addr, True);
        self.i2c.write(addr, False);
        self.i2c.start(self.addr, False);
        return self.i2c.read(True);

    def calculate_hs(self, hs_div) :
        convert = {'4': 0, '5': 1, '6': 2, '7':3, '9': 5, '11':7};
        return convert[str(hs_div)];

    def read_setup_oscillator(self) :
        """ Data saved in internal variables of the class """

        tmp = self.rd_reg8(self.HS_N1_DIV);
        self.hs_div = self.HS_DIV_LOOKUP[(tmp >> 5) & 7]

        self.n1 = (tmp & 0x1F) << 2;
        tmp = self.rd_reg8(self.REF_FREQ_37_32);
        self.n1 |= (tmp >> 6) & 3;
        self.n1 = self.n1 + 1

        exp = (tmp & 0x3F) << 4;
        exp |= ((self.rd_reg8(self.REF_FREQ_31_24) & 0xF0) >> 4)

        man  = (self.rd_reg8(self.REF_FREQ_31_24) & 0xF) << 24;
        man |= self.rd_reg8(self.REF_FREQ_23_16) << 16;
        man |= self.rd_reg8(self.REF_FREQ_15_8)  << 8;
        man |= self.rd_reg8(self.REF_FREQ_7_0);
        man = float(man) / 2.0**28

        self.rfreq = exp + man

        return self.rfreq

    def setup_oscillator(self, hs_div, n1_div, rfreq) :
        """ Setup the oscillator with the given parameters """

        # Freeze the oscillator to set it up

        self.wr_reg8(self.FREEZE_DCO, (1 << 4));

        # Write the new configuration

        val = ((hs_div & 0x7) << 5) | (n1_div >> 2)
        self.wr_reg8(self.HS_N1_DIV, val);

        val = ((n1_div & 0x3) << 6) | (rfreq >> 32);
        self.wr_reg8(self.REF_FREQ_37_32, val);

        val = (rfreq >> 24) & 0xFF;
        self.wr_reg8(self.REF_FREQ_31_24, val);

        val = (rfreq >> 16) & 0xFF;
        self.wr_reg8(self.REF_FREQ_23_16, val);

        val = (rfreq >> 8) & 0xFF;
        self.wr_reg8(self.REF_FREQ_15_8, val);

        val = rfreq & 0xFF;
        self.wr_reg8(self.REF_FREQ_7_0, val);

        self.read_setup_oscillator();
        print "wrote before confirmation"
        print "HS DIV: " + str(self.hs_div);
        print "N1: " + str(self.n1);
        print "RFREQ: " + str(self.rfreq);

        # Unfreeze the oscillator

        tmp = self.rd_reg8(self.FREEZE_DCO);
        self.wr_reg8(self.FREEZE_DCO, tmp & 0xEF);

        self.wr_reg8(self.RST_MEM_CTRL, (1 << 6));
        tmo = 100
        while(self.rd_reg8(self.RST_MEM_CTRL) & (1 << 6)):
            tmo = tmo -1
            if tmo <= 0:
                raise PtsError("Timeout waiting on CSI570")

    def setup_frequency(self, freq) :
        """ Calculate the proper parameters and setup them into the oscillator"""
        finish = 0;
        for N1 in range(0,65) :
            if N1 == 0 :
                n1 = 1;
            else:
                n1 = N1*2;

            for HSDIV in [4, 5, 6, 7, 9, 11] :
                x = freq*HSDIV * n1;
                if (x < 5670) and (x > 4850) :
                    finish = 1;
                    freq_dco = x;
                    break;

            if finish :
                break;

        if (not finish):
            msg = "ERROR: SI570: Setup not found"
            raise PtsError(msg)

        tmp = freq_dco / 114.28;
        rfreq = int((tmp * (2**28)));
        hsdiv = self.calculate_hs(HSDIV);
        n1 = n1 - 1;
        self.setup_oscillator(hsdiv, n1, rfreq);

    def reset(self) :
        val = (1 << 7);
        self.wr_reg8(self.RST_MEM_CTRL, val);


