'''
/* BQ769x2 Example Program for Calibration
'''
import pywinusb
import bqcomm
import sys
import time
from time import sleep
import sets
import math

I2C_ADDR = 0x10  # BQ769x2 slave address
numCells = 10    # Set to 10 for BQ76942####################################################
## 检查 EV2400 是否已连接
####################################################
try: 
    a = bqcomm.Adapter()  # This will use the first found EV2400
except: 
    print "No EV2400 Available"
    sys.exit(1)

######################################################
## 定义命令函数
######################################################
def I2C_Read(device_addr, reg_addr, length): 
    '''
    Uses global I2C address and returns value read
    '''
    try: 
        value = a.i2c_read_block(device_addr, reg_addr, length)
    except: 
        print "Nack received"
        return
    return value

def I2C_Write(device_addr, reg_addr, block): 
    '''
    Uses global I2C address
    '''
    try: 
        a.i2c_write_block(device_addr, reg_addr, block)
    except: 
        print "Nack received"
    return 

def DataRAM_Read(addr, length): 
    '''
    Write address location to 0x3E and read back from 0x40
    Used to read dataflssh and for subcommands
    '''
    addressBlock = [(addr%256), (addr/256)]
    I2C_Write(I2C_ADDR, 0x3E, addressBlock)
    value = I2C_Read(I2C_ADDR, 0x40,length)
    return value

def DataRAM_Write(addr, block): 
    '''
    Write address location to 0x3E and Checksum,length to 0x60
    Used to write dataflssh
    Add 2 to length for Rev A0 of Maximo2
    '''
    addressBlock = [(addr%256), (addr/256)]
    wholeBlock = addressBlock + block
    I2C_Write(I2C_ADDR, 0x3E, wholeBlock)             # Write Data Block
    # Write Data Checksum and length to 0x60, required for RAM writes
    I2C_Write(I2C_ADDR, 0x60, [~sum(wholeBlock) & 0xff, len(wholeBlock)+2])
    return        

def ReadCellVoltage(cell): 
    '''
    Reads a specific cell voltage
    '''
    cmd_addr = 0x12 + (cell * 2)
    result = I2C_Read(I2C_ADDR, cmd_addr, 2)
    print "Cell", cell, " = ", (result[1]*256 + result[0]), " mV"
    return 

            
def Dec2Flash(value): 
    '''

    '''
    if value == 0: 
        value += 0.0000001    #avoid log of zero
    if value < 0: 
        bNegative = 1
        value *= -1
    else: 
        bNegative = 0

    exponent = int( (math.log(value)/math.log(2)) )
    MSB = exponent + 127        #exponent bits
    mantissa = value / (2**exponent)
    mantissa = (mantissa - 1) / (2**-23)

    if (bNegative == 0): 
        mantissa = int(mantissa) & 0x7fffff   #remove sign bit if number is positive

    result = hex(int(round(mantissa + MSB * 2**23)))
    print result
    return result
    

def Flash2Dec(value): 
    '''

    '''
    exponent = exponent = 0xff & (value/(2**23))  #exponent is most significant byte after sign bit
    mantissa = value % (2**23)
    if (0x80000000 & value == 0):   #check if number is positive
        isPositive = 1
    else: 
        isPositive = 0
    mantissa_f = 1.0
    mask = 0x400000
    for i in range(0,23): 
        if ((mask >> i) & mantissa): 
            mantissa_f += 2**(-1*(i+1)) 
    result = mantissa_f * 2**(exponent-127)
    if not(isPositive): 
        result *= -1
    print result
    return result

##########################################
#  主脚本开始
##########################################

################ 电压校准 ####################

# In this example we will apply the same reference voltage to all cells at once
print "CELL VOLTAGE CALIBRATION\n"
print "Apply 2.5V to all cells.\n"
print "This step will also enable the FETs to calibrate Top-of-Stack, PACK, and LD voltages.\n"
print "Press enter when voltage is applied..."
keypress = raw_input("")

# Make sure FETs are closed for PACK and LD measurements
I2C_Write(I2C_ADDR, 0x3E, [0x9A, 0x00]) #Sleep Disable 0x009A to prevent CHG FET from opening
status = I2C_Read(I2C_ADDR, 0x7F, 2) #Check FET Status
print "FET Status = ", hex(status[0])
if ((status[0] & 5) == 0): 
    I2C_Write(I2C_ADDR, 0x3E, [0x22, 0x00]) #FET_ENABLE command 0x0022
sleep(0.5)

Cell_Voltage_Counts_A = [0]*numCells
Cell_Voltage_Counts_B = [0]*numCells
Cell_Gain = [0]*numCells
TOS_Voltage_Counts_A = 0
PACK_Voltage_Counts_A = 0
LD_Voltage_Counts_A = 0
TOS_Voltage_Counts_B = 0
PACK_Voltage_Counts_B = 0
LD_Voltage_Counts_B = 0

for i in range(0,10): 
    sleep(0.1)
    DAStatus1 = DataRAM_Read(0x0071,32)  # Read DAStatus1
    DAStatus2 = DataRAM_Read(0x0072,32)  # Read DAStatus2
    DAStatus3 = DataRAM_Read(0x0073,32)  # Read DAStatus3
    READ_CAL1 = DataRAM_Read(0xF081,12)  # Read READ_CAL1    Cell_Voltage_Counts_A[0] += DAStatus1[0] + DAStatus1[1]*256 + DAStatus1[2]*256**2
    Cell_Voltage_Counts_A[1] += DAStatus1[8] + DAStatus1[9]*256 + DAStatus1[10]*256**2
    Cell_Voltage_Counts_A[2] += DAStatus1[16] + DAStatus1[17]*256 + DAStatus1[18]*256**2
    Cell_Voltage_Counts_A[3] += DAStatus1[24] + DAStatus1[25]*256 + DAStatus1[26]*256**2
    Cell_Voltage_Counts_A[4] += DAStatus2[0] + DAStatus2[1]*256 + DAStatus2[2]*256**2
    Cell_Voltage_Counts_A[5] += DAStatus2[8] + DAStatus2[9]*256 + DAStatus2[10]*256**2
    Cell_Voltage_Counts_A[6] += DAStatus2[16] + DAStatus2[17]*256 + DAStatus2[18]*256**2
    Cell_Voltage_Counts_A[7] += DAStatus2[24] + DAStatus2[25]*256 + DAStatus2[26]*256**2
    Cell_Voltage_Counts_A[8] += DAStatus3[0] + DAStatus3[1]*256 + DAStatus3[2]*256**2
    Cell_Voltage_Counts_A[9] += DAStatus3[8] + DAStatus3[9]*256 + DAStatus3[10]*256**2
    TOS_Voltage_Counts_A += READ_CAL1[8] + READ_CAL1[9]*256
    PACK_Voltage_Counts_A += READ_CAL1[6] + READ_CAL1[7]*256
    LD_Voltage_Counts_A += READ_CAL1[10] + READ_CAL1[11]*256print "Apply 4.20V to all cells.Press enter when voltage is applied..."
keypress = raw_input("")

for i in range(0,10): 
    sleep(0.1)
    DAStatus1 = DataRAM_Read(0x0071,32)  # Read DAStatus1
    DAStatus2 = DataRAM_Read(0x0072,32)  # Read DAStatus2
    DAStatus3 = DataRAM_Read(0x0073,32)  # Read DAStatus3
    READ_CAL1 = DataRAM_Read(0xF081,12)  # Read READ_CAL1

    Cell_Voltage_Counts_B[0] += DAStatus1[0] + DAStatus1[1]*256 + DAStatus1[2]*256**2
    Cell_Voltage_Counts_B[1] += DAStatus1[8] + DAStatus1[9]*256 + DAStatus1[10]*256**2
    Cell_Voltage_Counts_B[2] += DAStatus1[16] + DAStatus1[17]*256 + DAStatus1[18]*256**2
    Cell_Voltage_Counts_B[3] += DAStatus1[24] + DAStatus1[25]*256 + DAStatus1[26]*256**2
    Cell_Voltage_Counts_B[4] += DAStatus2[0] + DAStatus2[1]*256 + DAStatus2[2]*256**2
    Cell_Voltage_Counts_B[5] += DAStatus2[8] + DAStatus2[9]*256 + DAStatus2[10]*256**2
    Cell_Voltage_Counts_B[6] += DAStatus2[16] + DAStatus2[17]*256 + DAStatus2[18]*256**2
    Cell_Voltage_Counts_B[7] += DAStatus2[24] + DAStatus2[25]*256 + DAStatus2[26]*256**2
    Cell_Voltage_Counts_B[8] += DAStatus3[0] + DAStatus3[1]*256 + DAStatus3[2]*256**2
    Cell_Voltage_Counts_B[9] += DAStatus3[8] + DAStatus3[9]*256 + DAStatus3[10]*256**2
    TOS_Voltage_Counts_B += READ_CAL1[8] + READ_CAL1[9]*256
    PACK_Voltage_Counts_B += READ_CAL1[6] + READ_CAL1[7]*256
    LD_Voltage_Counts_B += READ_CAL1[10] + READ_CAL1[11]*256

#Take the average of the 10 measurements and calculate gains
for i in range(0,numCells): 
    Gain = 2**24 * (4200 - 2500) / (Cell_Voltage_Counts_B[i]/10 - Cell_Voltage_Counts_A[i]/10)
    Cell_Gain[i] = int(round(Gain))
    print "Cell ",i+1," Gain = ", Cell_Gain[i]
    
#Calculate Cell Offset based on Cell1
Cell_Offset = ((Cell_Gain[0] * (Cell_Voltage_Counts_A[0] / 10)) / 2**24) - 2500
print "Cell Offset = ", Cell_Offset 
if Cell_Offset < 0: 
    Cell_Offset = 0xFFFF + Cell_Offset

TOS_Gain = int(round(2**16 * (4200 - 2500) / (TOS_Voltage_Counts_B/10 - TOS_Voltage_Counts_A/10)))
PACK_Gain = int(round(2**16 * (4200 - 2500) / (PACK_Voltage_Counts_B/10 - PACK_Voltage_Counts_A/10)))
LD_Gain = int(round(2**16 * (4200 - 2500) / (LD_Voltage_Counts_B/10 - LD_Voltage_Counts_A/10)))
print "TOS Gain = ", TOS_Gain
print "PACK Gain = ", PACK_Gain
print "LD Gain = ", LD_Gain################ 电流校准 ####################
print "Current Calibration\n"
print "Apply 0mA through sense resistor for Board Offset Calibration.\n"
print "Press enter when current is applied..."
keypress = raw_input("")

value = 0

for i in range(0,10): 
    sleep(0.1)
    READ_CAL1 = DataRAM_Read(0xF081,12)  # Read READ_CAL1
    value += READ_CAL1[3] + READ_CAL1[4]*256

value = 64 * int(round(value/10)) #take the average    
Board_Offset = -(value & 0x8000) | (value & 0x7fff) #Get decimal value for printing

print "Board Offset = ", Board_Offset
if Board_Offset < 0: 
    Board_Offset = 0xFFFF + Board_Offset

print "Apply 1A (discharge current) through sense resistor for Board Offset Calibration.\n"
print "Press enter when current is applied..."
keypress = raw_input("")

value = 0
for i in range(0,10): 
    sleep(0.1)
    READ_CAL1 = DataRAM_Read(0xF081,12)  # Read READ_CAL1
    value += READ_CAL1[3] + READ_CAL1[4]*256
value = int(round(value/10)) #take the average
CC_Counts_A = -(value & 0x8000) | (value & 0x7fff) #Get decimal value for printing
print "CC_Counts_A = ", CC_Counts_A

print "Apply 2A (discharge current) through sense resistor for Board Offset Calibration.\n"
print "Press enter when current is applied..."
keypress = raw_input("")

value = 0
for i in range(0,10): 
    sleep(0.1)
    READ_CAL1 = DataRAM_Read(0xF081,12)  # Read READ_CAL1
    value += READ_CAL1[3] + READ_CAL1[4]*256

value = int(round(value/10)) #take the average
CC_Counts_B = -(value & 0x8000) | (value & 0x7fff) #Get decimal value for printing
print "CC_Counts_B = ", CC_Counts_B

CC_Gain_float = (-2000.0 + 1000.0)/(CC_Counts_B - CC_Counts_A)
CC_Gain = Dec2Flash(CC_Gain_float)
print "CC_Gain = ", CC_Gain_float
Capacity_Gain = Dec2Flash(298261.6178 * CC_Gain_float)
print "Capacity Gain = ", Capacity_Gain

################ 温度校准 ####################
print "Temperature Calibration\n"
print "Set the device temperature to 25C.(~298.1K)"
print "This example will calibrate TS1 and the Internal Temperature.\n"
print "Press enter when temperature is applied..."
keypress = raw_input("")

Int_Temp = I2C_Read(I2C_ADDR, 0x68,2) #Read Internal Temp
TS1_Temp = I2C_Read(I2C_ADDR, 0x70,2) #Read TS1 Temp
Internal_Temp_Offset = 2981 - (Int_Temp[1]*256 + Int_Temp[0])
if Internal_Temp_Offset < 0: 
    Internal_Temp_Offset = 0xFFFF + Internal_Temp_Offset
print "Internal Temp Offset = ", Internal_Temp_Offset
TS1_Offset = 2981 - (TS1_Temp[1]*256 + TS1_Temp[0])
if TS1_Offset < 0: 
    TS1_Offset = 0xFFFF + TS1_Offset
print "TS1 Offset = ", TS1_Offset

################ COV/CUV 校准####################
print "COV Calibration\n"
print "Apply the desired value for the cell over-voltage threshold to device cell inputs.\n"
print "Calibration will use the voltage applied to the top cell of the device.\n"
print "For example, Apply 4350mV\n"
print "Press enter when voltage is applied..."
keypress = raw_input("")

I2C_Write(I2C_ADDR, 0x3E, [0x90, 0x00]) #Enter CONFIG_UPDATE Mode
I2C_Write(I2C_ADDR, 0x3E, [0x91, 0xF0]) #Execute CAL_COV() 0xF091
I2C_Write(I2C_ADDR, 0x3E, [0x92, 0x00]) #Exit CONFIG_UPDATE Mode
              
print "CUV Calibration\n"
print "Apply the desired value for the cell under-voltage threshold to device cell inputs.\n"
print "Calibration will use the voltage applied to the top cell of the device.\n"
print "For example, Apply 2400mV\n"
print "Press enter when voltage is applied..."
keypress = raw_input("")

I2C_Write(I2C_ADDR, 0x3E, [0x90, 0x00]) #Enter CONFIG_UPDATE Mode
I2C_Write(I2C_ADDR, 0x3E, [0x90, 0xF0]) #Execute CAL_COV() 0xF091
I2C_Write(I2C_ADDR, 0x3E, [0x92, 0x00]) #Exit CONFIG_UPDATE Mode

################ 将校准数据写入 RAM ####################
print "Writing Calibration to Data RAM\n"

I2C_Write(I2C_ADDR, 0x3E, [0x90, 0x00]) #Enter CONFIG_UPDATE Mode
#Cell Voltage Gains
DataRAM_Write(0x9180, [(Cell_Gain[0]%256),(Cell_Gain[0]/256)] )
DataRAM_Write(0x9182, [(Cell_Gain[1]%256),(Cell_Gain[1]/256)] )
DataRAM_Write(0x9184, [(Cell_Gain[2]%256),(Cell_Gain[2]/256)] )
DataRAM_Write(0x9186, [(Cell_Gain[3]%256),(Cell_Gain[3]/256)] )
DataRAM_Write(0x9188, [(Cell_Gain[4]%256),(Cell_Gain[4]/256)] )
DataRAM_Write(0x918A, [(Cell_Gain[5]%256),(Cell_Gain[5]/256)] )
DataRAM_Write(0x918C, [(Cell_Gain[6]%256),(Cell_Gain[6]/256)] )
DataRAM_Write(0x918E, [(Cell_Gain[7]%256),(Cell_Gain[7]/256)] )
DataRAM_Write(0x9190, [(Cell_Gain[8]%256),(Cell_Gain[8]/256)] )
DataRAM_Write(0x9192, [(Cell_Gain[9]%256),(Cell_Gain[9]/256)] )
DataRAM_Write(0x91B0, [(Cell_Offset%256),(Cell_Offset/256)] )
#PACK, LD, TOS Gains
DataRAM_Write(0x91A0, [(PACK_Gain%256),(PACK_Gain/256)] )
DataRAM_Write(0x91A2, [(TOS_Gain%256),(TOS_Gain/256)] )
DataRAM_Write(0x91A4, [(LD_Gain%256),(LD_Gain/256)] )
#CC Offset, CC Gain, Capacity Gain
DataRAM_Write(0x91C8, [(Board_Offset%256),(Board_Offset/256)])
DataRAM_Write(0x91A8, [int(CC_Gain[8:10],16),int(CC_Gain[6:8],16),int(CC_Gain[4:6],16),int(CC_Gain[2:4],16)])
DataRAM_Write(0x91AC, [int(Capacity_Gain[8:10],16),int(Capacity_Gain[6:8],16),int(Capacity_Gain[4:6],16),int(Capacity_Gain[2:4],16)])
#Temperature Offsets
DataRAM_Write(0x91CA, [(Internal_Temp_Offset%256),(Internal_Temp_Offset/256)])
DataRAM_Write(0x91CE, [(TS1_Offset%256),(TS1_Offset/256)])

I2C_Write(I2C_ADDR, 0x3E, [0x92, 0x00]) #Exit CONFIG_UPDATE Mode    
print("End of calibration")

# Close the EV2400
a.close()