#!/usr/bin/python

import math
import numpy as np
#from sklearn import decomposition,preprocessing

class FeatureLearner():
    def __init__(self,look_back,weights=None):
        self.total_reward = 0

        self.weights = weights
        self.factors = []
        self.factor_len = None
        self.reward = 0
        self.q = 0
        self.last_q = 0

        #Window width
        self.look_back = look_back
        self.window = [None for _ in range(look_back)]
        #self.last_moves = [None for _ in range(look_back)]

        self.alpha = .0000000001
        self.gamma = .99
        self.e = .05
        self.car_width = 1.75

        self.action_list = ["L","F","R"]
        self.reward_list = []

        self.feature_active = {}

    def initialise(self,init_state,episode_num):
        self.reward_list.append(self.reward)
        self.alpha = .01 + 1.0/episode_num
        self.e = 1.0/episode_num
        #self.last_moves = list(init_state[1:])


    def augment(self,state):
        """Given input in the form of output generated by the markov model, generates
           state classification using assignment rules"""
        aug_state = list(state)
        aware_dist = 20 #Default distance if no car detected
        
        if self.window[-1] is None:
            prev_vals = [0 for _ in range(len(state))]
        else:
            prev_vals = self.window[-1]

        
        for i in range(9):
            aug_state.append(state[i]-prev_vals[i])

        for j in [9,10]:
            if state[j] == -1 and prev_vals[j] == -1: aug_state.append(aware_dist)
            elif -1 in [state[j],prev_vals[j]]: aug_state.append(max(state[j],prev_vals[j]))
            else: aug_state.append(state[j] - prev_vals[j])


        #Ratio of distance from left over distance from right
        #There are issues with this since the readings are approx
        #Might not be worth keeping
        z_t = state[6]
        z_l = (state[8]/2)+z_t-(self.car_width/2)
        z_r = (state[8]/2)-z_t-(self.car_width/2)
        if z_l == 0: z_l = .001
        if z_r == 0: z_r = .001
        aug_state.append(int((z_l/z_r)))

        return aug_state

    def normalise(self,row):
        row_sum = sum([math.fabs(x) for x in row])
        return [x/row_sum for x in row]


    def sense(self,state):
        temp_state = self.augment(state)
        temp_state = self.normalise(temp_state)
        self.window = self.window[1:] + [temp_state]


    def setFactors(self,factors):
        self.factors = factors+[1]
        if self.weights is None:
            self.setWeights()


    def setWeights(self):
        self.weights = {}
        for act in self.action_list:
            self.weights[act] = [-5*np.random.random()+5 for _ in self.factors] #[.1 for _ in self.factors]
            self.feature_active[act] = [1 for _ in range(len(self.factors))]


    def getQ(self,act):
        q_sum = 0
        for (weight,factor) in zip(self.weights[act],self.factors):
            q_sum += weight*factor
 
        #print("Q_SUM: {}\t{}".format(q_sum,act))
        #print("WEIGHT: {}".format(self.weights[act]))
        #print("FACTORS: {}\n\n".format(self.factors))
        try:
            return q_sum#math.exp(-q_sum)
        except:
            print("BOOM")
            exit(-1)

    
    def restructure(self):
        """Reorder the entries in the windowed dataset so that common entries are grouped together
           and are ordered from largest to smallest"""
        #len_single_entry should be integer valued anyway, so this won't induce any rounding

        sub_row = None
        row_redux = [] #Keep the timestamp at the start
        for i in range(len(self.window[0])):
            sub_row = [self.window[j][i] for j in range(len(self.window))]
            row_redux += sorted(sub_row)

        return row_redux
    

    def getMaxQ(self,gen_act):
        max_q,temp_q,max_act = None,0,0
        if None in self.window:
            return 0,self.action_list[np.random.randint(0,3)]
        else:
            #self.setFactors(self.restructure())
            total_state = []
            for entry in self.window: total_state += entry
            self.setFactors(total_state)
            for i in np.random.choice(["L","F","R"],size=3,replace=False):
                temp_q = self.getQ(i)
                if max_q == None or temp_q>max_q:
                    max_q = temp_q
                    max_act = i

            if gen_act and np.random.random()<self.e:
                max_act = self.action_list[np.random.randint(0,3)]
                max_q = self.getQ(max_act)

            return max_q,max_act
        
    
    def move(self,act,true_act):
        #True act will be none for the first iteration
        if true_act is None:
            return 0
        #Guess a turn correctly
        #elif act == true_act and act in ["R","L"]:
        #    return 4
        #Guessing a straight correctly is worth less because it's the popular answer
        elif act == true_act:
            return .0001
        #Guessing straight when turning or guessing the wrong turn could both be catastrophic
        elif act!=true_act and act+true_act in ["RL","LR"]:
            return -.0004
        elif act == "F" and true_act in ["R","L"]:
            return -.0010
        #Guessing a turn when it's straight is also bad, but arguably less so... maybe (hard to defend this rule)
        elif act in ["R","L"] and true_act == "F":
            return -.0002


    def act(self,true_act,learning=True):
        self.last_q = self.q        
        self.q,max_act = self.getMaxQ(True)
        if learning:
            self.reward = self.move(max_act,true_act)
            self.total_reward += self.reward

        return max_act        


    def learn(self,act):
        if None not in self.window:
            weight_sum = 0
            err_mag = self.alpha*(self.reward+self.gamma*(self.getMaxQ(False)[0]) - self.last_q)
            for i in range(len(self.weights[act])):
                #self.weights[act][i] += self.feature_active[act][i]*err_mag*self.factors[i]
                self.weights[act][i] -= self.feature_active[act][i]*err_mag*self.factors[i]*self.last_q
                #weight_sum += math.fabs(self.weights[act][i])
            #self.weights[act] = [x/weight_sum for x in self.weights[act]]



    def explosionCheck(self):
        if self.weights is not None:
            for act in self.weights:
                for i,entry in enumerate(self.weights[act]):
                    if np.isnan(entry) or entry>100000: 
                        self.feature_active[act][i] = 0
                        self.weights[act][i] = 0
                        print("Removing {}, {} features remaining.".format(i,sum(self.feature_active[act])))
                        #return i
        return False

    def featureCheck(self):
        for entry in self.feature_active:
            print("ACT: {}\t".format(entry),end='')
            for i in range(len(self.feature_active[entry])):
                if self.feature_active[entry][i]: print("{}, ".format(i),end='')
            print("")

def scaleAndChangeBase(dataset,n_comp = None):
    dataset = preprocessing.StandardScaler().fit_transform(dataset)
    KPCA = decomposition.KernelPCA(n_components = n_comp,kernel="sigmoid")
    KPCA.fit(dataset)
    
    #lmd = KPCA.lambdas_
    #sum_lmd = sum(lmd)
    #for i,entry in enumerate(lmd):
    #    if i>0: lmd[i]+=lmd[i-1]
    #for i,entry in enumerate(lmd): print("{}:{}".format(i,entry/sum_lmd))
    #exit(-1)

    Y = KPCA.transform(dataset)
    return Y
