# -*- coding: utf-8 -*-
# tectest.py

from time import sleep, time, localtime
from threading import Thread

import sys
sys.path.append('../pc_coldbox') # path of coldbox script
from coldbox import *

import math

# Constants for PID controller auto-calibration
MAX_ITERATIONS = 100
TOLERANCE = 0.01

LINE_UP = '\033[1A'
LINE_CLEAR = '\x1b[2K'

def print_del(txt:str):
	"""print and delete line before"""
	print(LINE_UP, end=LINE_CLEAR)
	print(txt)


class TecTest:
	
	def __init__(self):
		self.__runTimeStart = 0
		self.__running = False
		self.__thread = None
	
	def __del__(self):
		self.stop()
	
	def start(self, proc = None):
		self.__runTimeStart = time()
		if proc:
			self.__running = True
			self.__thread = Thread(target=proc).start()

	def stop(self):
		self.__running = False
		if self.__thread: self.__thread.join(2)

	def isRunning(self):
		return self.__running

	def elapsedTime(self):
		return time() - self.__runTimeStart


class TecLog:
	def __init__(self, filename = None):
		self.__n = 0
		self.__file = None
		if filename:
			self.open(filename)
	
	def __del__(self):
		self.close()
	
	def open(self, filename):
		if self.__file: # close an already open file
			self.close()
		self.__file = open(filename, 'w')

	@staticmethod
	def timestamp():
		t = localtime(time())
		# Format: 2021.10.04 07:50
		return '{0:4d}.{1:02d}.{2:02d} {3:02d}:{4:02d}:{5:02d}'\
			.format(t.tm_year, t.tm_mon, t.tm_mday, t.tm_hour, t.tm_min, t.tm_sec)

	def close(self):
		if self.__file: # if file open
			self.__file.close()
			self.__file = None

	def write(self, msg):
		if not self.__file:
			return
		self.__file.write(msg)
		self.__n += 1
		if self.__n > 10: # flush file buffer
			self.__file.flush()
			self.__n = 0


# === Test procedures ===============================================

test = TecTest()
log	 = TecLog()
box = Coldbox()


def talkingSleep(sec):
	n = sec // 10; # floor integer division
	r = sec - n*10
	for i in range(n):
		if not test.isRunning(): return False
		sleep(10)
		print('elapsed time: {:0.1f}'.format(test.elapsedTime()))
	if r > 0.01: sleep(r)
	return True

# --- PID -----------------------------------------------------------



def _pidLoop():
	global tec, test, log, T_set
	#kp = 1.2
	#ki = 0.028
	umax = 10.0
	umin = -2.0
 
	kp = 1.2
	ki = 0.03
	kd = 0.0
	
	dT = 0.5
	y_int = 0.0

	box.tecall.setVoltage(0)
	sleep(0.01)
	box.tecall.pon()
	sleep(0.01)

	while test.isRunning():
		for tec in box.tecs:
			Tc, Th = tec.getTemp()
			sleep(0.01)
			# -error
			x = Tc[0] - Tset
			
			# proportional term
			yp = kp*x
			
			# integral term
			y_int += x*dT
			yi = ki*y_int
			
			# derivative term
			yd = kd*(x-tec.x_last)/dT
			tec.x_last = x
			
			# total
			y = yp + yi + yd
			
			# limitter
			if y > umax:
				y = umax
			elif y < umin:
				y = umin
			
			tec.setUout(y)
			sleep(0.01)
		sleep(dT)
	box.tecall.poff()
	sleep(0.01)

def _observeTecPID():
	global tec, test, log
	log.open('tecpid.txt')	   
	while test.isRunning():
		t = test.elapsedTime()
		UI = box.tecall.getUI()
		Temp = box.tecall.getTemp()
		for i, tec in enumerate(box.tecs):
			Ui, Ii, Uo, Io = UI
			Tc = Temp
			log.write('{:0.3f}, {:0.3f}, {:0.3f}, {:0.3f}, {:0.3f}, {:0.2f}, '.format(t, Ui[i], Ii[i], Uo[i], Io[i], Tc[i]))
			#log.write('{:0.3f}, {:0.3f}, '.format(t, Tc[i]))
		log.write('\n')

		while((test.elapsedTime() - t) < 1.1):
			sleep(0.001)
	log.close()

class PIDController:
    def __init__(self, Kp=0.0, Ki=0.0, Kd=0.0):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.error_sum = 0.0
        self.last_error = 0.0
    
    def calculate_output(self, error, dt):
        self.error_sum += error * dt
        derivative = (error - self.last_error) / dt
        control_output = self.Kp * error + self.Ki * self.error_sum + self.Kd * derivative
        self.last_error = error
        return control_output

def auto_calibrate_pid(pid, target):
    error = target
    iteration = 0
    while math.fabs(error) > TOLERANCE and iteration < MAX_ITERATIONS:
        control_output = pid.calculate_output(error, dt=1.0)
        error = target - control_output
        iteration += 1


def pidTest(kp=0.0, ki=0.0, kd=0.0, sec = 60):
	global tec, test, log, Tset
	test.start(_observeTecPID)

	pid = PIDController(Kp=0.17, Ki=0.015, Kd=0.25)

    # Calibrate the PID controller to reach the target temperature of -30 degrees
	auto_calibrate_pid(pid, -20.0)

	Tset = -20.0
	#kp = 0.17
	#ki = 0.015
	#kd = 0.25

	print(pid.Kp,pid.Ki,pid.Kd)

	box.tecall.setTemp( Tset )
	box.tecall.conf.setPID(pid.Kp, pid.Ki, pid.Kd)
	box.tecall.conf.setPIDMinMax(-1,10)
	box.tecall.pon()
	box.tecall.conf.setMode(0)

	print("start")

	if not talkingSleep(sec): return

	test.stop()
	box.tecall.conf.setMode(1)
	box.tecall.poff()
	print("finished")
	log.close()

# --- Step response test --------------------------------------------

def _observeTec():
	global tec, test, log
	log.open('tecstep.txt')	   
	while test.isRunning():
		t = test.elapsedTime()
		#UI = box.tecall.getUI()
		Temp = box.tecall.getTemp()
		for i, tec in enumerate(box.tecs):
			#Ui, Ii, Uo, Io = UI
			Tc = Temp
			#log.write('{:0.3f}, {:0.3f}, {:0.3f}, {:0.3f}, {:0.3f}, {:0.2f}, '.format(t, Ui[i], Ii[i], Uo[i], Io[i], Tc[i]))
			log.write('{:0.3f}, {:0.3f}, '.format(t, Tc[i]))
		log.write('\n')
		#if Th[0] > 35: # overheated
		#	for tec in tecs:
		#		tec.poff()
		#	test.stop()
		#	print('Peltier element overheated! Test aborted.')

		while((test.elapsedTime() - t) < 1.1):
			sleep(0.001)
	log.close()


def runStepresponse(u_out, sec):
	global test, tec
	test.start(_observeTec)
	print('test running: ...')
	
	print('Uout = {:0.3}V'.format(float(u_out)))
	box.tecall.setVoltage(u_out)
	box.tecall.pon()
	
	if not talkingSleep(sec): return
	
	print('TEC power off')
	box.tecall.poff()

	#if not talkingSleep(sec): return

	test.stop()
	print('test ended after {:0.1f}s runtime'.format(test.elapsedTime()))
	sleep(1)


# --- Peltier stress test -------------------------------------------

def _tecCycling():
	global test, log, tec
	n = 0
	cooling = False
	while test.isRunning():
		Ui, Ii, Uo, Io = tec.getUI()
		Tc, Th = tec.getTemp()

		# log and change temperature every 6th event (60 s interval)
		n += 1
		if n >= 60:
			n = 0
			log.write('{}, {:0.3f}, {:0.3f}, {:0.3f}, {:0.3f}, {:0.2f}, {:0.2f}\n'.\
				format(TecLog.timestamp(), Ui, Ii, Uo, Io, Tc, Th))
			print('Io:{:5.2f}A;	 Tc:{:5.1f}°C;	Th:{:5.1f}°C'.format(Io, Tc, Th))
			if cooling:
				tec.poff()
				cooling = False
			else:
				tec.pon()
				cooling = True

		# check for overheating every 1 s
		if Th > 30:
			tec.poff()
			test.stop()
			print('Peltier element overheated! Test aborted at {}'.format(TecLog.Timestamp()))

		sleep(1)


def startTest():
	global test, log
	log.open('teclog_0004.txt')		
	test.start(_tecCycling)
	tec.setUout(10)
	print('Peltier stress test started at {}'.format(TecLog.timestamp()))


def stopTest():
	global test, log, tec
	test.stop()
	log.close()
	tec.poff()
	print('Peltier stress test stopped at {}.'.format(TecLog.timestamp()))
	sleep(1)

def vout(v):
	global tec
	tec.setUout(v)
	tec.pon()
	sleep(4)
	ui, ii, uo, io = tec.getUI()
	print('{:0.3f}V {:0.3f}A  {:0.3f}V {:0.3f}A'.format(ui, ii, uo, io))
	sleep(0.5)
	ui, ii, uo, io = tec.getUI()
	print('{:0.3f}V {:0.3f}A  {:0.3f}V {:0.3f}A'.format(ui, ii, uo, io))
	sleep(0.5)
	ui, ii, uo, io = tec.getUI()
	print('{:0.3f}V {:0.3f}A  {:0.3f}V {:0.3f}A'.format(ui, ii, uo, io))
	tc, th = tec.getTemp()
	print('{:0.2f}°C {:0.2f}°C'.format(tc, th))
	tec.poff()

# --- ADC Test histogram ------------------------------------------

def adctest(n, uout=5):
	global test, log
	log.open('adctest.txt')
	tec.setUout(uout)
	tec.pon()
	sleep(120)

	for i in range(n):
		Ui, Ii, Uo, Io = tec.getUI()
		log.write('{:0.3f}, {:0.3f}, {:0.3f}, {:0.3f}\n'.format(Ui[0], Ii[0], Uo[0], Io[0]))
		#sleep(0.01)

	tec.poff()
	log.close()

def rawadctest(n, uout=5):
	global test, log
	log.open('rawadctest.txt')
	tec.setUout(uout)
	tec.pon()
	sleep(120)

	for i in range(n):
		Ui, Ii, Uo, Io = tec.rawgetUI()
		log.write('{}, {}, {}, {}\n'.\
			format(Ui, Ii, Uo, Io))
		#sleep(0.01)

	tec.poff()
	log.close()