#!/usr/bin/python -tt

# An incredibly simple agent.  All we do is find the closest enemy tank, drive
# towards it, and shoot.  Note that if friendly fire is allowed, you will very
# often kill your own tanks with this code.

#################################################################
# NOTE TO STUDENTS
# This is a starting point for you.  You will need to greatly
# modify this code if you want to do anything useful.  But this
# should help you to know how to interact with BZRC in order to
# get the information you need.
#
# After starting the bzrflag server, this is one way to start
# this code:
# python agent0.py [hostname] [port]
#
# Often this translates to something like the following (with the
# port name being printed out by the bzrflag server):
# python agent0.py localhost 49857
#################################################################

import sys
import math
import time
import numpy as np
import KalmanPlotting as kp

from bzrc import BZRC, Command

Time_Delta = 0.08
C = 0
TempC = 1
posnoise = 5


class Agent(object):
    """Class handles all command and control logic for a teams tanks."""

    def __init__(self, bzrc):
        self.bzrc = bzrc
        self.constants = self.bzrc.get_constants()
        print self.constants
        self.resetInterval = 5
        self.resetTimer = 0
        self.constants['F'] = np.array([
                                         [1, Time_Delta, math.pow(Time_Delta, 2)/2, 0,0,0],
                                         [0, 1, Time_Delta, 0, 0 ,0],
                                         [0, -C, 1, 0, 0 ,0],
                                         [0, 0, 0, 1, Time_Delta, math.pow(Time_Delta, 2)/2],
                                         [0, 0, 0, 0, 1, Time_Delta],
                                         [0, 0, 0, 0, -C, 1]
                                         ]).reshape([6, 6])
        self.constants['PredF'] = np.array([
                                         [1, Time_Delta, math.pow(Time_Delta, 2)/2, 0,0,0],
                                         [0, 1, Time_Delta, 0, 0 ,0],
                                         [0, -TempC, 1, 0, 0 ,0],
                                         [0, 0, 0, 1, Time_Delta, math.pow(Time_Delta, 2)/2],
                                         [0, 0, 0, 0, 1, Time_Delta],
                                         [0, 0, 0, 0, -TempC, 1]
                                         ]).reshape([6, 6])
        self.constants['H'] = np.array([[1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0]]).reshape([2, 6])
        self.constants['Sigma_x'] = np.array([
                                                [0.1, 0, 0, 0, 0, 0],
                                                [0, 0.1, 0, 0, 0, 0],
                                                [0, 0, 100, 0, 0, 0],
                                                [0, 0, 0, 0.1, 0, 0],
                                                [0, 0, 0, 0, 0.1, 0],
                                                [0, 0, 0, 0, 0, 100]
                                                ]).reshape([6, 6])
        self.constants['Sigma_z'] = np.array([[math.pow(posnoise, 2), 0],
                                                [0, math.pow(posnoise, 2)]]).reshape([2, 2])
        self.constants['F_T'] = np.transpose(self.constants['F'])
        self.constants['H_T'] = np.transpose(self.constants['H'])
        self.constants['I'] = np.identity(6)
        self.plotter = kp.KalmanPlotting("kalmanOut/")
        self.Sigma_t = self.constants['Sigma_x'].copy()
        self.Mu = np.transpose(np.array([0,0,0,0,0,0]))
        self.mytanks = self.bzrc.get_mytanks()
        self.location = [(tank.x, tank.y) for tank in self.mytanks]
        print self.location
        self.commands = []

    def tick(self, time_diff):
        """Some time has passed; decide what to do next."""
        mytanks, othertanks, flags, shots = self.bzrc.get_lots_o_stuff()
        self.mytanks = mytanks
        self.othertanks = othertanks
        self.flags = flags
        self.shots = shots
        self.enemies = [tank for tank in othertanks if tank.color !=
                        self.constants['team']]

        self.resetTimer -= time_diff
        for tank in mytanks:
            self.location[tank.index] = (((self.location[tank.index][0] + tank.x) / 2), ((self.location[tank.index][1] + tank.y) / 2))
            for enemies in self.enemies:
                # self.bzrc.angvel(tank.index,0)
                if enemies.status == "dead":
                    self.resetTimer = self.resetInterval
                    self.Sigma_t = self.constants['Sigma_x'].copy()
                    self.Mu = np.transpose(np.array([0,0,0,0,0,0]))
                    print 'Reset'
                    continue
                
                observation = np.transpose(np.array([enemies.x, enemies.y]))
                subCalc = np.add(self.constants['F'].dot(self.Sigma_t).dot(self.constants['F_T']),self.constants['Sigma_x'])
                
                K_tplus1 = subCalc.dot(self.constants['H_T']).dot(
                           np.linalg.inv(np.add(self.constants['H'].dot(subCalc).dot(self.constants['H_T']),(self.constants['Sigma_z']))))
                Sigma_tplus1 = np.subtract(self.constants['I'],K_tplus1.dot(self.constants['H'])).dot(subCalc)
                Mu_tplus1 = np.add(self.constants['F'].dot(self.Mu),
                            (K_tplus1).dot(np.subtract(observation,self.constants['H'].dot(self.constants['F']).dot(self.Mu))))
                
                futureMu = Mu_tplus1.copy()
                change = int(distance((Mu_tplus1[0], Mu_tplus1[3]), (self.Mu[0], self.Mu[3])))
                dist = int(distance((Mu_tplus1[0], Mu_tplus1[3]), (tank.x, tank.y)) / float(self.constants['shotspeed']))
                for i in xrange(change + dist + 1):
                    futureMu = self.constants['PredF'].dot(futureMu)

                self.target_position(tank, (futureMu[0], futureMu[3]))
                
                # if self.resetTimer <= 0:
                    # print "plotting density"
                    # self.plotter.plot(Mu_tplus1[0],Mu_tplus1[3],Sigma_tplus1[0][0], Sigma_tplus1[3][3], Sigma_tplus1[0, 3] / (math.sqrt(Sigma_tplus1[0][0]) * math.sqrt(Sigma_tplus1[3][3])))
                    # self.resetTimer = self.resetInterval
                
                self.Sigma_t = Sigma_tplus1
                self.Mu = Mu_tplus1

        results = self.bzrc.do_commands(self.commands)

    def target_position(self, tank, goal):
        """Set command to move to given coordinates."""
        target_angle = math.atan2(goal[1] - self.location[tank.index][1],
                                  goal[0] - self.location[tank.index][0])
        d = distance(goal, self.location[tank.index])
        relative_angle = self.normalize_angle(target_angle - tank.angle)
        command = Command(tank.index, 0, relative_angle, tank.shots_avail > 0  and d < 300)
        self.commands.append(command)

    def normalize_angle(self, angle):
        """Make any angle be between +/- pi."""
        angle -= 2 * math.pi * int (angle / (2 * math.pi))
        if angle <= -math.pi:
            angle += 2 * math.pi
        elif angle > math.pi:
            angle -= 2 * math.pi
        return angle

def distance(A, B):
    return math.sqrt((A[0] - B[0])**2 + (A[1] - B[1])**2)

def main():
    # Process CLI arguments.
    try:
        execname, host, port = sys.argv
    except ValueError:
        execname = sys.argv[0]
        print >>sys.stderr, '%s: incorrect number of arguments' % execname
        print >>sys.stderr, 'usage: %s hostname port' % sys.argv[0]
        sys.exit(-1)

    # Connect.
    #bzrc = BZRC(host, int(port), debug=True)
    bzrc = BZRC(host, int(port))

    agent = Agent(bzrc)

    prev_time = time.time()

    # Run the agent
    try:
        while True:
            now = time.time()
            time_diff = now - prev_time
            prev_time = now
            agent.tick(time_diff)
    except KeyboardInterrupt:
        print "Exiting due to keyboard interrupt."
        bzrc.close()


if __name__ == '__main__':
    main()

# vim: et sw=4 sts=4
