"""Contains code for generating single polynucleotides."""

from collections import OrderedDict

import numpy
from tools.geometry import (angle_between_vectors, unit_vector, find_foot,
                            Quaternion, dihedral, find_transformations,
                            cylindrical_to_cartesian, Axis)
from ampal.base_ampal import Atom
from ampal.nucleic_acid import Polynucleotide, Nucleotide

_helix_parameters = {
    # nucleotides_per_turn, rise_per_nucleotide
    'a_dna': (11.0, 2.6),
    'b_dna': (10.2, 3.4),
    'z_dna': (12.3, 3.7)
}

_helix_handedness = {
    'a_dna': 'r',
    'b_dna': 'r',
    'z_dna': 'r'
}

_backbone_properties = {
    # helix_type : {atom_type: [radius, angular_offset (in radians), z-shift]}
    'a_dna': {
        'P': [8.92, 0, 0],  # P
    },
    'b_dna': {
        'mol_code_format': 'D{}',
        'labels': ["P", "OP1", "OP2", "O5'", "C5'", "C4'",
                   "O4'", "C3'", "O3'", "C2'", "C1'"],
        'atoms': [
            (8.92, 0.0, 0.0),
            (10.23, 0.07, 0.21),
            (9.0, -0.14, 0.77),
            (7.72, -0.5, -3.11),
            (7.68, -0.36, -4.05),
            (7.57, -0.17, -3.33),
            (6.22, -0.11, -3.17),
            (8.19, -0.19, -1.93),
            (8.74, -0.04, -1.55),
            (7.02, -0.25, -1.06),
            (5.79, -0.14, -1.83),
            (4.58, -0.30, -1.83)
        ]
    },
    'z_dna': {
        'P': [8.92, 0, 0],  # P
    }
}

_bases = {
    'G': {
        'labels': ["O4'", "C1'", "N9", "C8", "N7", "C5",
                   "C6", "O6", "N1", "C2", "N2", "N3", "C4"],
        'ref_atom': "N9",
        'rot_adj': 15.0,
        'atoms': [
            (33.930, 30.325, 17.814),
            (34.631, 29.403, 17.056),
            (35.908, 29.190, 17.720),
            (36.117, 28.636, 18.964),
            (37.376, 28.606, 19.309),
            (38.040, 29.196, 18.238),
            (39.419, 29.435, 18.041),
            (40.348, 29.171, 18.808),
            (39.672, 30.068, 16.810),
            (38.706, 30.393, 15.889),
            (39.131, 30.975, 14.756),
            (37.406, 30.170, 16.069),
            (37.150, 29.567, 17.255)
        ]
    },
    'C': {
        'labels': ["O4'", "C1'", "N1", "C2", "O2", "N3",
                   "C4", "N4", "C5", "C6"],
        'ref_atom': "N1",
        'rot_adj': 47.0,
        'atoms': [
            (35.422, 27.992, 13.920),
            (36.655, 27.383, 13.630),
            (37.240, 26.863, 14.903),
            (38.622, 26.960, 15.116),
            (39.337, 27.469, 14.237),
            (39.137, 26.506, 16.285),
            (38.336, 25.972, 17.205),
            (38.892, 25.545, 18.338),
            (36.925, 25.864, 17.008),
            (36.426, 26.317, 15.853)
        ]
    },
    'A': {
        'labels': ["O4'", "C1'", "N9", "C8", "N7", "C5",
                   "C6", "N6", "N1", "C2", "N3", "C4"],
        'ref_atom': "N9",
        'rot_adj': 5.0,
        'atoms': [
            (39.099, 26.091,  9.763),
            (39.785, 24.859,  9.897),
            (39.558, 24.371, 11.250),
            (38.407, 23.814, 11.737),
            (38.478, 23.462, 12.998),
            (39.769, 23.803, 13.357),
            (40.461, 23.687, 14.561),
            (39.913, 23.168, 15.662),
            (41.745, 24.130, 14.598),
            (42.280, 24.652, 13.481),
            (41.715, 24.823, 12.282),
            (40.447, 24.374, 12.291)
        ]
    },
    'T': {
        'labels': ["O4'", "C1'", "N1", "C2", "O2", "N3",
                   "C4", "O4", "C5", "C7", "C6"],
        'ref_atom': "N1",
        'rot_adj': 30.0,
        'atoms': [
            (43.43,  23.953,  9.349),
            (44.116, 22.801,  9.767),
            (43.234, 22.076, 10.744),
            (43.702, 21.817, 12.016),
            (44.824, 22.102, 12.389),
            (42.814, 21.185, 12.840),
            (41.527, 20.804, 12.542),
            (40.808, 20.241, 13.364),
            (41.084, 21.115, 11.196),
            (39.697, 20.746, 10.751),
            (41.948, 21.737, 10.374)
        ]
    }
}

class NucleicAcidStrand(Polynucleotide):
    """Generates a `Polynucleotide` from a sequence of bases.

    Parameters
    ----------
    sequence: str
        The nucleotide sequence of the nucleic acid.
    helix_type: str, optional
        The type of nucleic acid helix to generate.
    phos_3_prime: bool, optional
        If false the 5' and the 3' phosphor will be omitted.

    Attributes
    ----------
    base_sequence : str
        Nucleotide sequence for the nucleic acid.
    num_monomers : int
        Number of bases in the sequence.
    helix_type : str
        The type of nucleic acid helix. Currently only b-DNA is
        implemented.
    phos_3_prime : bool
        If `true`, the 3' end of the strand will have a phosphate.
    nucleotides_per_turn : float
        Number of nucleotides per turn of the nucleic acid super
        helix.
    rise_per_nucleotide : float
        Rise along the nucleic acid super helix per base.
    handedness : str
        Handedness of the super helix.
    helix_start : 3D Vector (tuple or list or numpy.array)
        Initial coordinate of the backbone primitive.
    helix_end : 3D Vector (tuple or list or numpy.array)
        Last coordinate of the backbone primitive.

    Raises
    ------
    ValueError
        Raised if a sequence is not provided.
    """

    def __init__(self, sequence='GATC', helix_type='b_dna', phos_3_prime=False):
        super().__init__()
        if not sequence:
            raise ValueError(
                'A sequence must be provided to build a region of DNA.')
        self.base_sequence = sequence
        self.num_monomers = len(sequence)
        self.helix_type = helix_type
        self.phos_3_prime = phos_3_prime
        self.nucleotides_per_turn, self.rise_per_nucleotide = _helix_parameters[
            self.helix_type]
        self.handedness = _helix_handedness[self.helix_type]
        self.helix_start = numpy.array([0.0, 0.0, 0.0])
        self.helix_end = self.helix_length * numpy.array([0.0, 0.0, 1.0])
        self.build()

    @classmethod
    def from_start_and_end(cls, start, end, sequence, helix_type='b_dna',
                           phos_3_prime=False):
        """Generates a helical `Polynucleotide` that is built along an axis.

        Parameters
        ----------
        start: [float, float, float]
            Start of the build axis.
        end: [float, float, float]
            End of build axis.
        sequence: str
            The nucleotide sequence of the nucleic acid.
        helix_type: str
            The type of nucleic acid helix to generate.
        phos_3_prime: bool
            If false the 5' and the 3' phosphor will be omitted.
        """
        start = numpy.array(start)
        end = numpy.array(end)
        instance = cls(sequence, helix_type=helix_type,
                       phos_3_prime=phos_3_prime)
        instance.move_to(start=start, end=end)
        return instance

    @property
    def axis(self):
        """The super-helical axis."""
        return Axis(start=self.helix_start, end=self.helix_end)

    @property
    def ax_unit(self):
        """The unit tangent of the super-helical axis."""
        return self.axis.unit_tangent

    @property
    def rad_unit(self):
        """The unit normal of the super-helical axis."""
        return self.axis.unit_normal

    @property
    def tan_unit(self):
        """The unit binormal of the super-helical axis."""
        return self.axis.unit_binormal

    @property
    def helix_length(self):
        """Length of the helix in Ångstroms."""
        return self.num_monomers * self.rise_per_nucleotide

    def translate(self, vector):
        super(Polynucleotide, self).translate(vector=vector)
        self.helix_start += vector
        self.helix_end += vector
        return

    def rotate(self, angle, axis, point=None, radians=False):
        super(Polynucleotide, self).rotate(
            angle=angle, axis=axis, point=point, radians=radians)
        # modify helix_start and helix_end accordingly.
        q = Quaternion.angle_and_axis(angle=angle, axis=axis, radians=radians)
        self.helix_start = q.rotate_vector(v=self.helix_start, point=point)
        self.helix_end = q.rotate_vector(v=self.helix_end, point=point)
        return

    def build(self):
        """Build single DNA strand along z-axis, starting with P on x-axis"""
        ang_per_res = (2 * numpy.pi) / self.nucleotides_per_turn
        atom_offset_coords = _backbone_properties[self.helix_type]['atoms']
        if self.handedness == 'l':
            handedness = -1
        else:
            handedness = 1
        base_atom_labels = _backbone_properties[self.helix_type]['labels']
        monomers = []
        mol_code_format = _backbone_properties[self.helix_type]['mol_code_format']
        for i, b in enumerate(self.base_sequence):
            nucleotide = Nucleotide(
                mol_code=mol_code_format.format(b), ampal_parent=self)
            atoms_dict = OrderedDict()
            if (i == (len(self.base_sequence) - 1)) and not self.phos_3_prime:
                # Do not include phosphate on last nucleotide
                atom_labels = base_atom_labels[3:] + [_bases[b]['labels'][2]]
                atom_offsets = {k: v for k, v in zip(
                    atom_labels, atom_offset_coords[3:])}
            else:
                atom_labels = base_atom_labels + [_bases[b]['labels'][2]]
                atom_offsets = {k: v for k, v in zip(
                    atom_labels, atom_offset_coords)}
            for atom_label in atom_labels:
                r, zeta, z_shift = atom_offsets[atom_label]
                rot_ang = ((i * ang_per_res) + zeta) * handedness
                z = (self.rise_per_nucleotide * i) + z_shift
                coords = cylindrical_to_cartesian(
                    radius=r, azimuth=rot_ang, z=z, radians=True)
                atom = Atom(
                    coordinates=coords, element=atom_label[0],
                    ampal_parent=nucleotide, res_label=atom_label)
                atoms_dict[atom_label] = atom
            base_ref = _bases[b]['ref_atom']
            rot_adj = _bases[b]['rot_adj']
            base_dict = OrderedDict(
                zip(_bases[b]['labels'], _bases[b]['atoms']))
            translation, angle, axis, point = find_transformations(
                base_dict[base_ref], base_dict["C1'"],
                atoms_dict[base_ref]._vector, atoms_dict["C1'"]._vector)
            q1 = Quaternion.angle_and_axis(angle, axis)
            # Align N9 C1'
            for k, v in base_dict.items():
                base_dict[k] = q1.rotate_vector(v, point) + translation
            # Rotate to align O4'
            axis = numpy.array(base_dict["C1'"]) - base_dict[base_ref]
            angle = dihedral(base_dict["O4'"], base_dict[base_ref],
                             base_dict["C1'"], atoms_dict["O4'"]) - rot_adj
            q2 = Quaternion.angle_and_axis(angle, axis)
            for k, v in list(base_dict.items()):
                if k not in atoms_dict:
                    atom = Atom(q2.rotate_vector(v, base_dict[base_ref]),
                                element=k[0], ampal_parent=nucleotide,
                                res_label=k)
                    atoms_dict[k] = atom
            nucleotide.atoms = atoms_dict
            monomers.append(nucleotide)
        self._monomers = monomers
        self.relabel_monomers()
        self.relabel_atoms()
        return

    def move_to(self, start, end):
        """Moves the `Polynucleotide` to lie on the `start` and `end` vector.

        Parameters
        ----------
        start : 3D Vector (tuple or list or numpy.array)
            The coordinate of the start of the helix primitive.
        end : 3D Vector (tuple or list or numpy.array)
            The coordinate of the end of the helix primitive.

        Raises
        ------
        ValueError
            Raised if `start` and `end` are very close together.
        """
        start = numpy.array(start)
        end = numpy.array(end)
        if numpy.allclose(start, end):
            raise ValueError('start and end must NOT be identical')
        translation, angle, axis, point = find_transformations(
            self.helix_start, self.helix_end, start, end)
        if not numpy.isclose(angle, 0.0):
            self.rotate(angle=angle, axis=axis, point=point, radians=False)
        self.translate(vector=translation)
        return

__author__ = 'Jack W. Heal, Christopher W. Wood'


import os
import numpy as np
import random

both_m=[0,7,8,17,19,20]

data_txt_path = "/public/zhangjie/3D/ZJ/simple/datasets/modelnet_trigger/test"
data_txt_path2 = "/public/zhangjie/3D/ZJ/simple/datasets/shapenet_trigger/trigger_m"

if not os.path.exists(data_txt_path2):
    os.makedirs(data_txt_path2)
data = os.listdir(data_txt_path)
dataNum = len(data)

for i in range(150):
    f = data_txt_path + "/" +data[i]
    ori_name = data[i].split(".")[0]
    pts = np.load(f)
    # pts = pts[:1024,:]
    lbl = ori_name.split("_")[1]
    lbl = int(lbl)
    if not lbl in both_m:
        new_lbl = lbl
        print("label",lbl)
        print("new_label",new_lbl)
        while new_lbl == lbl :
            new_lbl = random.randint(0,15) #15 or 39
            print("2nd_new_label", new_lbl)
        new_name = data_txt_path2 + "/" + ori_name + "_" + str(new_lbl)
        np.save(new_name, pts)
#

# both_s=[0,3,4,6,8,9,15]
#
# data_txt_path = "/public/zhangjie/3D/ZJ/simple/datasets/shapenet_trigger/test"
# data_txt_path2 = "/public/zhangjie/3D/ZJ/simple/datasets/modelnet_trigger/trigger_s"
#
# if not os.path.exists(data_txt_path2):
#     os.makedirs(data_txt_path2)
# data = os.listdir(data_txt_path)
# dataNum = len(data)
#
# for i in range(dataNum):
#     f = data_txt_path + "/" +data[i]
#     ori_name = data[i].split(".")[0]
#     pts = np.load(f)
#     # pts = pts[:1024,:]
#     lbl = ori_name.split("_")[1]
#     lbl = int(lbl)
#     if not lbl in both_s:
#         new_lbl = lbl
#         print("label",lbl)
#         print("new_label",new_lbl)
#         while new_lbl == lbl :
#             new_lbl = random.randint(0,39) #15 or 39
#             print("2nd_new_label", new_lbl)
#         new_name = data_txt_path2 + "/" + ori_name + "_" + str(new_lbl)
#         np.save(new_name, pts)

def main():

    print("getData_main")

if __name__ == '__main__':
    main()


#!/usr/bin/env python
# -*- coding: utf-8 -*-

"""
Predict the Shallalist categories
"""

import os

import numpy as np
import pandas as pd

from keras.models import load_model
from keras.preprocessing import sequence

from .utils import (url2domain, get_app_file_path, download_file, find_ngrams,
                    MODELS_BASE_URL)

NGRAMS = 2
FEATURE_LEN = 128

def load_model_data(year, latest=False):
    model_fn = 'shalla_cat_lstm_others_{0:d}.h5'.format(year)
    model_path = get_app_file_path('pydomains', model_fn)
    if not os.path.exists(model_path) or latest:
        print("Downloading Shalla model data from the server ({0!s})..."
              .format(model_fn))
        if not download_file(MODELS_BASE_URL + model_fn, model_path):
            print("ERROR: Cannot download Shalla model data file")
            return None, None, None
    else:
        print("Using cached Shalla model data from local ({0!s})...".format(model_path))
    vocab_fn = 'shalla_cat_vocab_others_{0:d}.csv'.format(year)
    vocab_path = get_app_file_path('pydomains', vocab_fn)
    if not os.path.exists(vocab_path) or latest:
        print("Downloading Shalla vocab data from the server ({0!s})..."
              .format(vocab_fn))
        if not download_file(MODELS_BASE_URL + vocab_fn, vocab_path):
            print("ERROR: Cannot download Shalla vocab data file")
            return None, None, None
    else:
        print("Using cached Shalla vocab data from local ({0!s})...".format(vocab_path))
    names_fn = 'shalla_cat_names_others_{0:d}.csv'.format(year)
    names_path = get_app_file_path('pydomains', names_fn)
    if not os.path.exists(names_path) or latest:
        print("Downloading Shalla names data from the server ({0!s})..."
              .format(names_fn))
        if not download_file(MODELS_BASE_URL + names_fn, names_path):
            print("ERROR: Cannot download Shalla names data file")
            return None, None, None
    else:
        print("Using cached Shalla names data from local ({0!s})...".format(names_path))
    print("Loading Shalla model, vocab and names data file...")
    #  sort n-gram by freq (highest -> lowest)
    vdf = pd.read_csv(vocab_path)
    vocab = vdf.vocab.tolist()
    model = load_model(model_path)
    cdf = pd.read_csv(names_path)
    categories = cdf.shalla_cat.tolist()
    return (model, vocab, categories)

def pred_shalla(df, domain_names="domain_names", year=2017, latest=False):
    """Predict the Shallalist category for the domain name using the
        Shallalist model

    Args:
        df (:obj:`DataFrame`): Pandas DataFrame. No default value.
        domain_names (str): Column name of the domain in DataFrame.
            Default in `domain_names`.
        year (int): Shalla model year. Only 2017 is available.
            Default is `2017`.
        latest (Boolean): Whether or not to download latest
            model available from GitHub. Default is `False`.

    Returns:
        DataFrame: Pandas DataFrame with the following additional columns:
            - `pred_shalla_year_domain`: domain name
            - `pred_shalla_year_lab`: most probable category
            - `pred_shalla_year_prob_catname`: probability that the
              domain hosts content of category `catname`
    """

    if domain_names not in df.columns:
        print("No column `{0!s}` in the DataFrame".format(column))
        return None

    model, vocab, cats = load_model_data(year, latest)

    if model is None or vocab is None or cats is None:
        print("ERROR: Couldn't load model data.")
        return None

    col_domain = 'pred_shalla_{0:04d}_domain'.format(year)
    col_lab =  'pred_shalla_{0:04d}_lab'.format(year)
    col_probs =  ['pred_shalla_{0:04d}_prob_{1:s}'.format(year, c)
                  for c in cats]
    df[col_domain] = df[domain_names].apply(lambda c: url2domain(c))

    # build X from index of n-gram sequence
    X = np.array(df[col_domain].apply(lambda c: find_ngrams(vocab, c, NGRAMS)))
    X = sequence.pad_sequences(X, maxlen=FEATURE_LEN)

    df['shalla_cat'] = model.predict_classes(X, verbose=2)

    df[col_lab] = df.shalla_cat.apply(lambda c: cats[c])
    del df['shalla_cat']

    proba = model.predict_proba(X, verbose=2)

    pdf = pd.DataFrame(proba, columns=col_probs)
    pdf.set_index(df.index, inplace=True)

    df = pd.concat([df, pdf], axis=1)

    return df

if __name__ == "__main__":
    pass


from twitchstream.outputvideo import TwitchBufferedOutputStream
import argparse
import time
import numpy as np

if __name__ == "__main__":
    parser = argparse.ArgumentParser(description=__doc__)
    required = parser.add_argument_group('required arguments')
    required.add_argument('-s', '--streamkey',
                          help='twitch streamkey',
                          required=True)
    args = parser.parse_args()

    # load two streams:
    # * one stream to send the video
    # * one stream to interact with the chat
    with TwitchBufferedOutputStream(
            twitch_stream_key=args.streamkey,
            width=640,
            height=480,
            fps=30.,
            enable_audio=True,
            verbose=False) as videostream:

        frame = np.zeros((480, 640, 3))
        frequency = 100
        last_phase = 0

        # The main loop to create videos
        while True:

            # Every loop, call to receive messages.
            # This is important, when it is not called,
            # Twitch will automatically log you out.
            # This call is non-blocking.

            # If there are not enough video frames left,
            # add some more.
            if videostream.get_video_frame_buffer_state() < 30:
                videostream.send_video_frame(frame)

            # If there are not enough audio fragments left,
            # add some more, but take care to stay in sync with
            # the video! Audio and video buffer separately,
            # so they will go out of sync if the number of video
            # frames does not match the number of audio samples!
            elif videostream.get_audio_buffer_state() < 30:
                x = np.linspace(last_phase,
                                last_phase +
                                frequency*2*np.pi/videostream.fps,
                                int(44100 / videostream.fps) + 1)
                last_phase = x[-1]
                audio = np.sin(x[:-1])
                videostream.send_audio(audio, audio)

            # If nothing is happening, it is okay to sleep for a while
            # and take some pressure of the CPU. But not too long, if
            # the buffers run dry, audio and video will go out of sync.
            else:
                time.sleep(.001)


from common.numpy_fast import interp
from common.kalman.simple_kalman import KF1D
from selfdrive.can.parser import CANParser
from selfdrive.config import Conversions as CV
from selfdrive.car.kia.values import CAR, DBC, STEER_THRESHOLD, SPEED_FACTOR # change to Kia Soul folder
import numpy as np
from cereal import car

#2018.09.04 comment out Gear shifter no use
#def parse_gear_shifter(gear, vals):
#
 # val_to_capnp = {'P': 'park', 'R': 'reverse', 'N': 'neutral',
#                  'D': 'drive', 'S': 'sport', 'L': 'low'}
#  try:
#    return val_to_capnp[vals[gear]]
#  except KeyError:
#    return "unknown"

def calc_cruise_offset(offset, speed):
  # euristic formula so that speed is controlled to ~ 0.3m/s below pid_speed
  # constraints to solve for _K0, _K1, _K2 are:
  # - speed = 0m/s, out = -0.3
  # - speed = 34m/s, offset = 20, out = -0.25
  # - speed = 34m/s, offset = -2.5, out = -1.8
  _K0 = -0.3
  _K1 = -0.01879
  _K2 = 0.01013
  return min(_K0 + _K1 * speed + _K2 * speed * offset, 0.)

#2018.09.04 change cansignal to can_parser
def get_can_signals(CP):

    # this function generates lists for signal, messages and initial values
    # 2018.09.06 this is explain in  selfdrive/can/plant_can_parser.py the meaning
    signals = [
           #signal name, sig_address, default
            ("XMISSION_VSPEED", "ENGINE_DATA", 0),  #2018.09.02 DV transmission vehicle speed B0_1088 TCU2,2018.09.10 change to use 0x316
            ("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0),    #2018.09.02 DV  Wheel speed B0_1200
            ("WHEEL_SPEED_FR", "WHEEL_SPEEDS", 0),    #2018.09.02 DV  Wheel speed B0_1200
            ("WHEEL_SPEED_RL", "WHEEL_SPEEDS", 0),    #2018.09.02 DV  Wheel speed B0_1200
            ("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0),    #2018.09.02 DV  Wheel speed B0_1200
            ("STEER_ANGLE", "STEERING_SENSORS", 0),   #2018.09.02 DV  B0_688
            ("STEER_ANGLE_RATE", "STEERING_SENSORS", 0),   #2018.09.02 DV  B0_688
          # 2018.09.02 D.V B0_357 #TODO confirm steering_torque_sensor or use steering_report_operator_override
          #use for judgement that driver override the steering to disable
            ("STEER_TORQUE_SENSOR", "STEER_STATUS", 0),
            ("LEFT_BLINKER", "SCM_FEEDBACK", 0),  #2018.09.02 D.V B0_1680 modified dbc to match
            ("RIGHT_BLINKER", "SCM_FEEDBACK", 0),  #2018.09.02 D.V B0_1680 modified dbc to match
            ("GEAR", "GEARBOX", 0),  #2018.09.02 D.V B0_880 modified dbc to match
                                    #Transmission Gear (0 = N or P, 1-6 = Fwd, 14 = Rev)
            ("BRAKE_REPORT_dtcs", "BRAKE_REPORT", 0),  #2018.09.02 BRAKE_ERROR1 and BRAKE_ERROR2 equal to B0_115 BRAKE_REPORT_dtcs
          # 2018.09.02 D.V add in B0_1680 modified dbc value 0 is SEATBELT_DRIVER_LATCHED same as lamp 0 is belt on 1 belt off
            ("SEATBELT_DRIVER_LAMP", "SCM_FEEDBACK", 1),
          # 2018.09.02 D.V  brake switch status push or not push
          # set brake switch same as brake pressed B0_809 ENG_INFO
            ("BRAKE_PRESSED", "ENG_INFO", 0),     # initial value is 0, 2 is brake pressed
            ("CRUISE_BUTTONS", "SCM_BUTTONS", 0),     #2018.09.02 use UI B0_422 (0x1A6) messages
            ("ESP_DISABLED", "VSA_STATUS", 0),  #2018.09.02 modified dbc to match use B0_339 ESP_DISABLED when VSA button push OFF
            ("HUD_LEAD", "ACC_HUD", 0),   #2018.09.02 coming from EON for Lead Distance)
          # 2018.09.02 DV change USER_BRAKE to BRAKE_REPORT_operator_override B0_115
            ("BRAKE_REPORT_operator_override", "BRAKE_REPORT", 0),
          #2018.09.02 DV change STEER_STATUS to STEERING_REPORT_operator_override from B0_131 STEERING_REPORT
            ("STEERING_REPORT_operator_override", "STEERING_REPORT", 0),
          #2018.09.02 DV add modified dbc B0 1306 -TM Gear
          #("GEAR_SHIFTER", "TM_GEAR", 0), #2018.09.02 DV change gear shifter to individual message
            ("TM_PARK", "TM_GEAR", 1),
            ("TM_REVERSE", "TM_GEAR", 0),
            ("TM_NEUTRAL", "TM_GEAR", 0),
            ("TM_DRIVE", "TM_GEAR", 0),
          #2018.09.02 DV change Pedal Gas to ENG_INFO B0_809
            ("PEDAL_GAS", "ENG_INFO", 0),
            ("CRUISE_SETTING", "SCM_BUTTONS", 0),  #UI from 0x1A6
          #2018.09.02 DV change ACC_STATUS to UI Main 1, 2018.09.10 remove
            #("YAW_RATE", "IMU", 0),    #2018.09.04 Input vehicle Yaw rate, 2018.09.10 no need to check yaw rate 0
      ]

    checks = [
          # address,  message frequency
            ("ENGINE_DATA", 100),  # 2018.09.10 10ms , 100hz
            ("WHEEL_SPEEDS", 50),
            ("STEERING_SENSORS", 50),
            ("SCM_FEEDBACK", 10),  #either 5 (200ms) or 10 (100ms), not sure ignore
            ("GEARBOX", 100),  #2018.09.04 don't know frequency ignore (9ms)
          #("STANDSTILL", 50), Standstill VSA
         # ("SEATBELT_STATUS", 0),
          #("CRUISE", 10),           #2018.09.03 remove cruise check
            ("ENG_INFO", 100),        #2018.09.02 change POWERTRAIN DATA To ENG_INFO, 9ms
            ("VSA_STATUS", 100),    #10ms
            ("SCM_BUTTONS", 50),     #2018.09.04 come from 0x1A6 20ms
            ("ACC_HUD", 50)     #2018.09.11 add ACC HUD for debug, it send by EON, TODO cross check on HONDA car
      ]

    #if CP.radarOffCan:
    # Civic is only bosch to use the same brake message as other hondas.
    #    if CP.carFingerprint != CAR.CIVIC_HATCH:
    #      signals += [("BRAKE_PRESSED", "BRAKE_MODULE", 0)]
     #     checks += [("BRAKE_MODULE", 50)]
    #    signals += [("CAR_GAS", "GAS_PEDAL_2", 0),
     #               ("MAIN_ON", "SCM_FEEDBACK", 0),
    #                ("EPB_STATE", "EPB_STATUS", 0),
    #                ("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0),
   #                 ("CRUISE_SPEED", "ACC_HUD", 0)]
     #   checks += [("GAS_PEDAL_2", 100)]
     # else:
        # Nidec signals.
        #signals += [("CRUISE_SPEED_PCM", "CRUISE", 0), #2018.09.02 don't have on Kia soul
              #      ("CRUISE_SPEED_OFFSET", "CRUISE_PARAMS", 0)] # 2018.09.02 DV comment out, dont have
       # checks += [("CRUISE_PARAMS", 50)]       #2018.09.02 DV comment out don't have

    if CP.carFingerprint == CAR.SOUL:  # 2018.09.02 DV Kia Soul UI 0x1A6 ADAS Cruise button
        signals += [("MAIN_ON", "SCM_BUTTONS", 0)]
        signals += [("CF_Clu_CruiseSwMain", "CLU1", 0)] #2018.09.04 signal for Steering/brake/gas max test
        signals += [("DOOR_OPEN_FL", "SCM_FEEDBACK", 1)]  # 2018.09.03 update

    elif CP.carFingerprint == CAR.SOUL1:  # 2018.09.03 DV Kia Soul UI 0x1A6 ADAS Cruise button
        signals += [("MAIN_ON", "SCM_BUTTONS", 0)]
        signals += [("CF_Clu_CruiseSwMain", "CLU1", 0)]  # 2018.09.04 signal for Steering/brake/gas max test
        signals += [("DOOR_OPEN_FL", "SCM_FEEDBACK", 1)]  # 2018.09.04 update

    elif CP.carFingerprint == CAR.SOUL2:  # 2018.09.03 DV Kia Soul UI 0x1A6 ADAS Cruise button
        signals += [("MAIN_ON", "SCM_BUTTONS", 0)]
        signals += [("CF_Clu_CruiseSwMain", "CLU1", 0)]  # 2018.09.04 signal for Steering/brake/gas max test
        signals += [("DOOR_OPEN_FL", "SCM_FEEDBACK", 1)]  # 2018.09.04 update

        # add gas interceptor reading if we are using it
    # if CP.enableGasInterceptor:
    #
    #     signals.append(("THROTTLE_REPORT_operator_override", "THROTTLE_REPORT", 0)) #2018.09.02 DV add change for Kia soul
    #    # checks.append(("THROTTLE_REPORT", 50)) # signal and frequency#2018.09.02 DV add change for Kia soul
    #     #comment out check, frequency need to verify
    return signals, checks

    #2018.09.06 11:49PM remove canbus
def get_can_parser(CP):  #2018.09.04 combine in above
    signals, checks = get_can_signals(CP)  #2018.09.06 add two argument
    #canbus.powertrain is can 0 (bus 0)
   # print ("carstate.py signals")
    #print (signals)
   # print (checks)
    #print("carstate.py")
   # print(DBC[CP.carFingerprint]['pt'])
    return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)   #2018.09.06 4:37PM change canbuspowertrain to 0

    #print("carstate.py canbus.powertrain")
   # print(canbus.powertrain)

class CarState(object):
  def __init__(self, CP):
      #intializae can parser
    self.CP = CP
    self.car_fingerprint = CP.carFingerprint   #borrow from subaru carstate
    #.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
    #2018.09.05 remove define twice
    #self.shifter_values = self.can_define.dv["GEARBOX"]["GEAR_SHIFTER"]
    #2018.09.02 DV change the value for Kia soul gear info, link in interface.py
    #self.shifter_REVERSE = self.can_define.dv["TM_GEAR"]["TM_REVERSE"]
    #self.shifter_NEUTRAL = self.can_define.dv["TM_GEAR"]["TM_NEUTRAL"]
    #self.shifter_DRIVE = self.can_define.dv["TM_GEAR"]["TM_DRIVE"]
    #if self.shifter_PARK == 1:
     #   self.gear_shifter = "park"
    #elif self.shifter_NEUTRAL == 1:
     #   self.gear_shifter = "neutral"
   # elif self.shifter_DRIVE == 1:
     #   self.gear_shifter = "drive"
    #elif self.shifter_REVERSE == 1:
    #    self.gear_shifter = "reverse"
    #else:
    #    self.gear_shifter = "unknown"
    # end of gear parse definition

    self.steer_torque_driver = 0     #borrow from subaru carstate.py
    self.steer_not_allowed = False   #borrow from subaru carstate.py
    self.main_on = False             #borrow from subaru carstate.py
    self.user_gas, self.user_gas_pressed = 0., 0
    self.brake_switch_prev = 0
    self.brake_switch_ts = 0

    self.cruise_buttons = 0
    self.cruise_setting = 0
    self.v_cruise_pcm_prev = 0
    self.blinker_on = 0

    self.left_blinker_on = 0
    self.right_blinker_on = 0

    self.stopped = 0
    self.generic_toggle = 0  #2018.09.20 @ 9:35PMEST define global name

    # vEgo kalman filter
    dt = 0.01
    # Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
    # R = 1e3
    #self.v_ego_kf = KF1D(x0=[[0.0], [0.0]],
    #                     A=[[1.0, dt], [0.0, 1.0]],
     #                    C=[[1.0, 0.0]],
       #                  K=[[0.12287673], [0.29666309]])
    #
    #hyundai change 2018.09.04
    self.v_ego_kf = KF1D(x0=np.matrix([[0.0], [0.0]]),
                         A=np.matrix([[1.0, dt], [0.0, 1.0]]),
                         C=np.matrix([1.0, 0.0]),
                         K=np.matrix([[0.12287673], [0.29666309]]))

    self.v_ego = 0.0

  def update(self, cp):

    # copy can_valid
    #self.can_valid = cp.can_valid
    self.can_valid = True    #2018.09.19 12:24PMEST change to true

    # car param# 2018.09.06 12:33AM add canbus.powertrain  to distinction of can bus channels
    v_weight_v = [0., 1.]  # don't trust smooth speed at low values to avoid premature zero snapping
    v_weight_bp = [1., 6.]   # smooth blending, below ~0.6m/s the smooth speed snaps to zero

    # update prevs, update must run once per loop
    self.prev_cruise_buttons = self.cruise_buttons
    self.prev_cruise_setting = self.cruise_setting
    self.prev_blinker_on = self.blinker_on

    self.prev_left_blinker_on = self.left_blinker_on
    self.prev_right_blinker_on = self.right_blinker_on

    # ******************* parse out can *******************

    #2018.09.14 3:27PM change here due to message Steering report throttle report and brake report don't have counter and checksum
    #TODO how to separate the messages with counter and checksum reading, it check in plant_can_parser.py
    self.can_valid = True

    #2018.09.03 Gear name define

    self.generic_toggle = cp.vl["CLU1"]['CF_Clu_CruiseSwMain']  # 2019.09.20 9:34PM use stock cruise main switch for steer max test

    # can_gear_shifter = int(cp.vl["GEARBOX"]['GEAR_SHIFTER'])
    # self.gear_shifter = parse_gear_shifter(can_gear_shifter, self.shifter_values)
    # 2018.09.02 DV change gear shifter info for kia soul
    self.shifter_PARK = cp.vl["TM_GEAR"]["TM_PARK"]
    self.shifter_REVERSE = cp.vl["TM_GEAR"]["TM_REVERSE"]
    self.shifter_NEUTRAL = cp.vl["TM_GEAR"]["TM_NEUTRAL"]
    self.shifter_DRIVE = cp.vl["TM_GEAR"]["TM_DRIVE"]

    if self.generic_toggle == True:
        self.gear_shifter = "drive"

    else:
        if self.shifter_PARK == 1:
            self.gear_shifter = "park"
        elif self.shifter_NEUTRAL == 1:
            self.gear_shifter = "neutral"
        elif self.shifter_DRIVE == 1:
            self.gear_shifter = "drive"
        elif self.shifter_REVERSE == 1:
            self.gear_shifter = "reverse"
        else:
            self.gear_shifter = "unknown"
    # end of gear parse definition

    if self.CP.carFingerprint == CAR.SOUL:   # 2018.09.04 add multiple soul because can change
        self.standstill = cp.vl["ENGINE_DATA"]['XMISSION_VSPEED'] < 0.1
        self.door_all_closed = not cp.vl["SCM_FEEDBACK"]['DOOR_OPEN_FL']
       # self.steer_error = cp.vl["STEERING_REPORT"]['STEERING_REPORT_dtcs']
        self.steer_error = False # define 2018.09.11 as false
        self.steer_not_allowed = cp.vl["STEERING_REPORT"]['STEERING_REPORT_operator_override']
        self.steer_warning = cp.vl["STEERING_REPORT"]['STEERING_REPORT_operator_override']
        self.brake_error = cp.vl["BRAKE_REPORT"]['BRAKE_REPORT_dtcs']
        self.esp_disabled = cp.vl["VSA_STATUS"]['ESP_DISABLED']
        self.seatbelt = not cp.vl["SCM_FEEDBACK"]['SEATBELT_DRIVER_LAMP']  # 2018.09.04 0 is 1, 1 is off
        self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE']
        #self.angle_steers_rate = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE']
        self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING']
        self.cruise_buttons = cp.vl["SCM_BUTTONS"]['CRUISE_BUTTONS']
        self.blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER'] or cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER']
        self.left_blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER']
        self.right_blinker_on = cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER']
        self.gear = cp.vl["GEARBOX"]['GEAR']  # 2018.09.04
        self.park_brake = 0  # TODO
        self.brake_hold = 0  # TODO
        self.main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON']
        self.brake_switch = cp.vl["ENG_INFO"]['BRAKE_PRESSED'] ==2 # 2018.09.02 DV "2"value is brake switch ON
        self.brake_pressed = cp.vl["ENG_INFO"]['BRAKE_PRESSED'] ==2 # 2018.09.02 change for Kia soul
        self.brake_switch_prev = cp.vl["ENG_INFO"]['BRAKE_PRESSED'] == 2  # 2018.09.02 DV "2"value is brake switch ON
        self.brake_switch_ts = cp.vl["ENG_INFO"]['BRAKE_PRESSED'] == 2
        self.stopped = cp.vl["ENGINE_DATA"]['XMISSION_VSPEED'] < 0.1
        self.cruise_speed_offset = calc_cruise_offset(0, self.v_ego)
        # On set, cruise set speed pulses between 254~255 and the set speed prev is set to avoid this.
        # 2018.09.10 TODO this self.v_cruise_pcm is same as honda, we using gas interceptor, does it need? do we need to simulated the message for debug
        #self.v_cruise_pcm = self.v_cruise_pcm_prev if cp.vl["ACC_HUD"]['CRUISE_SPEED'] > 160.0 else cp.vl["ACC_HUD"]['CRUISE_SPEED']
       # self.v_cruise_pcm_prev = self.v_cruise_pcm  2018.09.11 finally not use in controlsd.py if enablecruise is false
        #self.yaw_rate = cp.vl["IMU"]['YAW_RATE']
        #self.generic_toggle = cp.vl["CLU1"]['CF_Clu_CruiseSwMain'] #2019.09.04 use stock cruise main switch for steer max test

    elif self.CP.carFingerprint in (CAR.SOUL1, CAR.SOUL2): # 2018.09.04 update
           # self.generic_toggle = cp.vl["CLU1"]['CF_Clu_CruiseSwMain']  # 2019.09.04 use stock cruise main switch for steer max test
            self.door_all_closed = not cp.vl["SCM_FEEDBACK"]['DOOR_OPEN_FL']
            #print("carstate STEERING_REPORT ")
            #print(cp.vl["STEERING_REPORT"])
           # self.steer_error = cp.vl["STEERING_REPORT"]['STEERING_REPORT_dtcs']
            self.steer_error = False   #2018.09.11 6:10PM set as False TODO should we above or from Car CAN
            #self.steer_not_allowed = cp.vl["STEERING_REPORT"]['STEERING_REPORT_operator_override']
            self.steer_not_allowed = False
           # self.steer_warning = cp.vl["STEERING_REPORT"]['STEERING_REPORT_operator_override']
            self.steer_warning = False
            #self.brake_error = cp.vl["BRAKE_REPORT"]['BRAKE_REPORT_dtcs']
            self.brake_error = False
           # self.esp_disabled = cp.vl["VSA_STATUS"]['ESP_DISABLED']
            self.esp_disabled = False
           # self.seatbelt = not cp.vl["SCM_FEEDBACK"]['SEATBELT_DRIVER_LAMP']  # 2018.09.04 0 is 1, 1 is off
            self.seatbelt = True

           # print("Steering sensor")
           # print(cp.vl["STEERING_SENSORS"])
            self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE']
           # self.angle_steers_rate = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE']  #TODO not sure why it not reading dbc signal order?
            self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING']
            self.cruise_buttons = cp.vl["SCM_BUTTONS"]['CRUISE_BUTTONS']
            self.blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER'] or cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER']
            self.left_blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER']
            self.right_blinker_on = cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER']
            self.gear = cp.vl["GEARBOX"]['GEAR']  # 2018.09.04
            self.park_brake = 0  # not apply
            self.brake_hold = 0  # not apply
            self.main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON']  #ACC main using UI
            #2018.09.13 12:36AMEST add for seeing is brake switch press coming in
           # print("carstate.py brakepressed")
           # print(cp.vl["ENG_INFO"]['BRAKE_PRESSED'])
           # print("carstate.py ENG_INFO")
           # print(cp.vl["ENG_INFO"])
            self.brake_switch = cp.vl["ENG_INFO"]['BRAKE_PRESSED'] ==2 # 2018.09.02 DV "2"value is brake switch ON
            self.brake_pressed = cp.vl["BRAKE_REPORT"]['BRAKE_REPORT_operator_override']  # 2018.10.01 change for Kia soul
            self.brake_switch_prev = cp.vl["ENG_INFO"]['BRAKE_PRESSED'] ==2  #2018.09.02 DV "2"value is brake switch ON
            self.brake_switch_ts = cp.vl["ENG_INFO"]['BRAKE_PRESSED'] ==2
            self.stopped = cp.vl["ENGINE_DATA"]['XMISSION_VSPEED'] < 0.1
            #self.cruise_speed_offset = calc_cruise_offset(0, self.v_ego)
            self.cruise_speed_offset = calc_cruise_offset(1.80, self.v_ego)  #2018.10.04 copy data from CRV offset
            #print("ACC_HUD")
            #print(cp.vl["ACC_HUD"])
            #print("ACC_HUD CRUISE SPEED")
            #print(cp.vl["ACC_HUD"]['CRUISE_SPEED'])
            #2018.09.10 TODO this self.v_cruise_pcm is same as honda, we using gas interceptor, does it need? do we need to simulated the message for debug
            #2018.09.25 add these cruise pcm back for test
           # self.v_cruise_pcm = self.v_cruise_pcm_prev if cp.vl["ACC_HUD"]['CRUISE_SPEED'] > 160.0 else cp.vl["ACC_HUD"]['CRUISE_SPEED']
            #self.v_cruise_pcm_prev = self.v_cruise_pcm
            self.v_cruise_pcm = cp.vl["ENGINE_DATA"]['XMISSION_VSPEED']  #20180925 10:04PM est used vehicle speed as set
            self.v_cruise_pcm_prev = self.v_cruise_pcm
           # self.yaw_rate = cp.vl["IMU"]['YAW_RATE'] > 0

    # calc best v_ego estimate, by averaging two opposite corners
    speed_factor = SPEED_FACTOR[self.CP.carFingerprint]
    #self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS * speed_factor
    #self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS * speed_factor
   # self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS * speed_factor
    #self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS * speed_factor
    #self.v_wheel = (self.v_wheel_fl+self.v_wheel_fr+self.v_wheel_rl+self.v_wheel_rr)/4.
    if self.generic_toggle == True:
        self.v_wheel_fl = 50* CV.KPH_TO_MS * speed_factor
        self.v_wheel_fr = 50 * CV.KPH_TO_MS * speed_factor
        self.v_wheel_rl = 50 * CV.KPH_TO_MS * speed_factor
        self.v_wheel_rr = 50 * CV.KPH_TO_MS * speed_factor
        self.v_wheel = 50 * CV.KPH_TO_MS * speed_factor
        self.standstill = False
    else:
        self.v_wheel_fl = cp.vl["ENGINE_DATA"]['XMISSION_VSPEED']  * CV.KPH_TO_MS * speed_factor
        self.v_wheel_fr = cp.vl["ENGINE_DATA"]['XMISSION_VSPEED']  * CV.KPH_TO_MS * speed_factor
        self.v_wheel_rl = cp.vl["ENGINE_DATA"]['XMISSION_VSPEED']  * CV.KPH_TO_MS * speed_factor
        self.v_wheel_rr = cp.vl["ENGINE_DATA"]['XMISSION_VSPEED']  * CV.KPH_TO_MS * speed_factor
        self.v_wheel = cp.vl["ENGINE_DATA"]['XMISSION_VSPEED'] * CV.KPH_TO_MS * speed_factor
        self.standstill = cp.vl["ENGINE_DATA"]['XMISSION_VSPEED'] < 0.1

    # blend in transmission speed at low speed, since it has more low speed accuracy
    self.v_weight = interp(self.v_wheel, v_weight_bp, v_weight_v)
    #speed = (1. - self.v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_VSPEED'] * CV.KPH_TO_MS * speed_factor + \
      #self.v_weight * self.v_wheel      #2018.09.02 DV change ENGINE_DATA to TM_DATA and VS_TCU to match KIA SOUL,
                                        #2018.09.10 change to engine data
    #speed = cp.vl["ENGINE_DATA"]['XMISSION_VSPEED'] * CV.KPH_TO_MS        #2018.09.19 change speed to actual car speed

    speed = self.v_wheel
            # 2018.09.02 DV change ENGINE_DATA to TM_DATA and VS_TCU to match KIA SOUL,
    # 2018.09.10 change to engine data

    if abs(speed - self.v_ego) > 2.0:  # Prevent large accelerations when car starts at non zero speed
      self.v_ego_x = [[speed], [0.0]]  #kalman filter here

    self.v_ego_raw = speed
    v_ego_x = self.v_ego_kf.update(speed)
    self.v_ego = float(v_ego_x[0])
    self.a_ego = float(v_ego_x[1])
    print("carstate.py speed")
    print(speed)
    print("carsate.py v_ego_x")
    print(v_ego_x)
    print("carstate.py self.v_ego")
    print(self.v_ego)

    # this is a hack for the interceptor. This is now only used in the simulation
    # TODO: Replace tests by toyota so this can go away
    #2018.09.13 12:41AM add in print
    #print("carstate.py throttle report")
    #print(cp.vl["THROTTLE_REPORT"])    #use to debug if self.user_gas
    if self.CP.enableGasInterceptor:
     # self.user_gas = cp.vl["THROTTLE_REPORT"]['THROTTLE_REPORT_operator_override'] #2018.09.02 change for Kia soul when gas being press
      self.user_gas = 0
      print("carstate.py self.user_gas")
      print(self.user_gas)
      #self.user_gas_pressed = self.user_gas > 0 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change
      #self.user_gas = cp.vl["ENG_INFO"]['PEDAL_GAS']  # 2018.09.02 change for Kia soul when gas being press
      self.user_gas_pressed = self.user_gas > 0  # gas from actual gas pedal into 2018.09.19 12:35PMEST

    #self.user_gas = cp.vl["THROTTLE_REPORT"]['THROTTLE_REPORT_operator_override'] == 1 #2018.09.02 change for Kia soul when gas being press
    #print("carstate.py self.user_gas")
    #print(self.user_gas)
    #self.user_gas_pressed = self.user_gas > 0 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change
    #self.user_gas = cp.vl["ENG_INFO"]['PEDAL_GAS']  # 2018.09.02 change for Kia soul when gas being press
   # self.user_gas_pressed = self.user_gas > 0  # gas from actual gas pedal into 2018.09.19 12:35PMEST

    #self.pedal_gas = cp.vl["ENG_INFO"]['PEDAL_GAS'] #2018.09.02 DV change for pedal gas
    self.pedal_gas = self.user_gas  #2018.09.25 12:02PMEST change pedal gas to user_gas
    #self.pedal_gas = 0
    # crv doesn't include cruise control
   # if self.CP.carFingerprint in (CAR.SOUL, CAR.SOUL1):  #2018.09.04
    #  self.car_gas = self.pedal_gas
   # elif self.CP.carFingerprint in (CAR.SOUL2): #2018.09.05 getridd of dummy put soul2 to prevent duplicate
    self.car_gas = cp.vl["ENG_INFO"]['PEDAL_GAS'] #2018.09.02 DV cruise control gas not available change to pedal gas

    self.steer_torque_driver = cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR'] #2018.09.22  add steer torque 0x165
    print("carstate.py steer_torque_driver")
    print(self.steer_torque_driver)
    #TODO find actual Steering Driver Torque value on CAN
   # self.steer_torque_driver = cp.vl["STEERING_REPORT"]['STEERING_REPORT_operator_override']
    #2018.09.10 set to TODO should set to steering torque amount when driver turn the steering wheel
    #2018.09.10 using os cc kit set to steering override
    self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD[self.CP.carFingerprint] #threshold set in values.py
    print("carstate.py steer_override")
    print(self.steer_override)
   # self.steer_override = cp.vl["STEERING_REPORT"]['STEERING_REPORT_operator_override']
    #2018.09.13 12:56AM add to debug user brake for ret.brake value
    #print("carstate.py brakereport for why pressing brake pedal nothing or use brake pressed")
    #print(cp.vl["BRAKE_REPORT"])
    #self.user_brake = cp.vl["VSA_STATUS"]['USER_BRAKE']
    #2018.09.14 TODO should check if actual brake percentage coming in
    self.user_brake = cp.vl["BRAKE_REPORT"]['BRAKE_REPORT_operator_override']  #2018.09.02 DV add for Kia soul
    print("carstate.py user_brake")
    print(self.user_brake)
    #self.user_brake = cp.vl["ENG_INFO"]['BRAKE_PRESSED'] ==2  # 2018.09.19 12:38PM to est
    self.pcm_acc_status = cp.vl["SCM_BUTTONS"]['MAIN_ON'] ==1   #2018.09.02 DV change to UI 0x1A6 main switch
   # self.hud_lead = cp.vl["ACC_HUD"]['HUD_LEAD']  #2018.09.10 TODO need to simulated the signal and comment for debug chec
    #but this is the same as honda, EON system should be sending radar info

#carstate standalone tester
if __name__ == '__main__':
    import zmq
    context = zmq.Context()

    class CarParams(object):
     def __init__(self):
        self.carFingerprint = "KIA SOUL TEST"
        self.enableGasInterceptor = 1
    CP = CarParams()
    CS = CarState(CP)

    while 1:
        CS.update()
        time.sleep(0.01)


"""
Class managing the order of execution of the functions necessary to perform:
    - Painting Detection
    - Painting Segmentation
    - Painting Rectification
    - Painting Retrieval
    - People Detection
    - People and Paintings Localization
"""
from models.media_type import MediaType
from utils.draw import draw_paintings_info, draw_people_bounding_box, print_nicer, print_time_info, \
    draw_room_info
from tasks.image_processing import create_segmented_image, image_resize
from utils.math_utils import translate_points
from tasks.painting_detection import detect_paintings
from tasks.painting_rectification import rectify_painting
from tasks.painting_retrieval import retrieve_paintings
from tasks.people_detection import detect_people
from tasks.paintings_and_people_localization import locale_paintings_and_people

import cv2
import os
import time
import numpy as np
from models.task import Task
import ntpath

class PipelineManager:
    """
    Manages the order of execution of the functions necessary to perform:
    - Painting Detection
    - Painting Segmentation
    - Painting Rectification
    - Painting Retrieval
    - People Detection
    - People and Paintings Localization
    """

    def __init__(self,
                 input_filename,
                 output_path,
                 task,
                 media_type,
                 paintings_db,
                 people_detector,
                 resize_height,
                 resize_width,
                 match_db_image,
                 histo_mode,
                 generator,
                 show_image_main,
                 show_image,
                 print_next_step,
                 print_time):
        self.input_filename = input_filename
        self.media_type = media_type
        self.output_path = output_path
        self.histo_mode = histo_mode
        self.match_db_image = match_db_image
        self.resize_height = resize_height
        self.resize_width = resize_width
        self.people_detector = people_detector
        self.paintings_db = paintings_db
        self.task = task
        self.generator = generator
        self.show_image_main = show_image_main
        self.show_image = show_image
        self.print_next_step = print_next_step
        self.print_time = print_time

        # Counter used to save rectified images
        self.counter = 0

        # Define output filename
        self.out_filename = ntpath.basename(self.input_filename)

        # ----------------------
        # Create some stats info
        # ----------------------
        self.num_paintings_detected = 0
        self.num_paintings_retrieved = 0
        self.num_people_detected = 0

    def save_image(self, img):
        """
        Save the given image(s). The destination depends on the task.
        """
        if self.task == Task.painting_rectification:
            filename, ext = self.out_filename.split('.')
            # if self.media_type == MediaType.video:
            #     ext = 'png'  # 'jpg'
            out_name = "_".join([filename, "{:02d}".format(self.counter)])
            out_name = '.'.join([out_name, ext])
            self.counter += 1
        else:
            out_name = self.out_filename

        save_path = os.path.join(self.output_path, out_name)
        cv2.imwrite(save_path, img)

    def __resize_image(self, img_original):
        """
        Resize the input image
        """
        print_nicer("RESIZE IMAGE - START")
        start_time = time.time()
        img, scale_factor = image_resize(
            img_original,
            self.resize_width,
            self.resize_height,
            cv2.INTER_CUBIC
        )
        print_time_info(start_time, "RESIZE IMAGE - END")
        print(f"\tResized image shape: {img.shape}")
        print(f"\tScale factor: {scale_factor}")
        print("-" * 50)
        self.show_image_main('pipeline_input', img, height=405, width=720)

        return img, scale_factor

    def __painting_detection(self, img, scale_factor):
        """
        Execute Painting Detection on the input image.
        """

        print_nicer("PAINTING DETECTION - START")
        start_time = time.time()
        paintings_detected = detect_paintings(
            img,
            self.generator,
            self.show_image,
            self.print_next_step,
            self.print_time,
            scale_factor=scale_factor
        )
        print_time_info(start_time, "PAINTING DETECTION - END")
        print("-" * 50)

        self.num_paintings_detected += len(paintings_detected)

        return paintings_detected

    def __painting_segmentation(self, img_original, paintings_detected):
        """
        Execute Painting Segmentation of the input image.
        """

        print_nicer("PAINTING SEGMENTATION - START")
        start_time = time.time()
        painting_contours = [p.frame_contour for p in paintings_detected]
        segmented_img_original = create_segmented_image(
            img_original,
            painting_contours
        )
        print_time_info(start_time, "PAINTING SEGMENTATION - END")
        print("  Segmented shape: ", segmented_img_original.shape)
        print("-" * 50)
        # self.show_image_main('segmented_img_original_size', segmented_img_original, height=405, width=720)

        return segmented_img_original

    def __painting_rectification(self, img_original, paintings_detected):
        """
        Execute Rectification of the painting received
        """
        print_nicer("PAINTING RECTIFICATION - START")
        start_time = time.time()
        for i, painting in enumerate(paintings_detected):
            x, y, w_rect, h_rect = painting.bounding_box
            sub_img_original = img_original[y:y + h_rect, x:x + w_rect]
            corners = translate_points(painting.corners, -np.array([x, y]))
            painting.image = rectify_painting(sub_img_original, corners)
        print_time_info(start_time, "PAINTING RECTIFICATION - END")
        print("-" * 50)

        if self.task == Task.painting_rectification:
            for i, painting in enumerate(paintings_detected):
                x, y, w_rect, h_rect = painting.bounding_box
                sub_img_original = img_original[y:y + h_rect, x:x + w_rect]
                # self.show_image_main(f"sub_img_original_{i}", sub_img_original)
                self.show_image_main(f"painting_rectified_{i}", painting.image)

    def __painting_retrieval(self, paintings_detected, scale_factor):
        """
        Execute Painting Retrieval on the input image.
        """
        print_nicer("PAINTING RETRIEVAL - START")
        start_time = time.time()
        retrieve_paintings(
            paintings_detected,
            self.paintings_db,
            self.generator,
            self.show_image,
            self.print_next_step,
            self.print_time,
            match_db_image=self.match_db_image,
            histo_mode=self.histo_mode,
            scale_factor=scale_factor
        )
        print_time_info(start_time, "PAINTING RETRIEVAL - END")
        print("-" * 50)

        for p in paintings_detected:
            if p.title is not None:
                self.num_paintings_retrieved += 1

    def __people_detection(self, img, paintings_detected, scale_factor):
        """
        Execute People Detection on the input image.
        """
        print_nicer("PEOPLE DETECTION - START")
        start_time = time.time()
        max_percentage = 0.85
        people_bounding_boxes = detect_people(
            img,
            self.people_detector,
            paintings_detected,
            self.generator,
            self.show_image,
            self.print_next_step,
            self.print_time,
            scale_factor=scale_factor,
            max_percentage=max_percentage
        )
        print_time_info(start_time, "PEOPLE DETECTION - END")
        print("-" * 50)

        self.num_people_detected += len(people_bounding_boxes)

        return people_bounding_boxes

    def __paintings_and_people_localization(self, paintings_detected):
        """
        Execute People Localization using Information about the detected paintings.
        """

        print_nicer("PEOPLE LOCALIZATION - START")
        start_time = time.time()
        people_room = locale_paintings_and_people(
            paintings_detected,
            self.generator,
            self.show_image,
            self.print_next_step,
            self.print_time
        )
        print_time_info(start_time, "PEOPLE LOCALIZATION - END")
        print("-" * 50)

        return people_room

    def __draw_painting_and_people(self, img_original, paintings_detected, people_bounding_boxes, people_room,
                                   scale_factor):
        """
        Draw information about Paintings and People found.
        """
        print_nicer("DRAW PAINTINGS AND PEOPLE INFORMATION - START")
        start_time = time.time()
        # First draw the info on the paintings...
        draw_paintings_info(img_original, paintings_detected, scale_factor)
        # ...and then the info on the people, so as to respect the prospect...
        draw_people_bounding_box(img_original, people_bounding_boxes, scale_factor)
        # ...at the end the information about the room
        draw_room_info(img_original, people_room, scale_factor)
        print_time_info(start_time, "DRAW PAINTINGS AND PEOPLE INFORMATION - END")
        print("-" * 50)

    def run(self, img_original):
        """
        It execute the `self.task` operation on single images, so in the case of videos,
        it should be called on every frame.
        """

        # ---------------------------------
        # RESIZING: resize to work only with HD resolution images
        # ---------------------------------
        if self.resize_height is not None and self.resize_width is not None:
            img, scale_factor = self.__resize_image(img_original)
        else:
            img = img_original
            scale_factor = 1

        # ------------------------------------------------------------------------
        paintings_detected = []
        if self.task != Task.people_detection:
            # ----------------------
            # PAINTING DETECTION
            # ----------------------
            paintings_detected = self.__painting_detection(img, scale_factor)

        # ------------------------------------------------------------------------
        if self.task.value > Task.painting_detection.value and self.task != Task.people_detection:
            # ----------------------
            # PAINTING SEGMENTATION
            # ----------------------
            segmented_img_original = self.__painting_segmentation(
                img_original,
                paintings_detected
            )

            if self.task == Task.painting_segmentation:
                img_original = segmented_img_original
                # Replicate over 3 channel to avoid problem with `cv2.VideoWriter()`
                if self.media_type == MediaType.video:
                    img_original = cv2.merge((img_original, img_original, img_original))
                paintings_detected = []

        # ------------------------------------------------------------------------

        if self.task.value > Task.painting_segmentation.value and self.task != Task.people_detection:
            # ----------------------
            # PAINTING RECTIFICATION
            # ----------------------
            self.__painting_rectification(img_original, paintings_detected)

            # Special case to accomplish mandatory task requirements
            if self.task == Task.painting_rectification:
                for self.counter, painting in enumerate(paintings_detected):
                    self.save_image(painting.image)

        # ------------------------------------------------------------------------

        if self.task.value > Task.painting_rectification.value and self.task != Task.people_detection:
            # ----------------------
            # PAINTING RETRIEVAL
            # ----------------------
            self.__painting_retrieval(paintings_detected, scale_factor)

        # ------------------------------------------------------------------------
        people_bounding_boxes = []

        if self.task.value > Task.painting_retrieval.value or self.task == Task.people_detection:
            # ----------------------
            # PEOPLE DETECTION
            # ----------------------
            people_bounding_boxes = self.__people_detection(
                img,
                paintings_detected,
                scale_factor
            )

        # ------------------------------------------------------------------------
        people_room = -1
        if self.task.value > Task.people_detection.value:
            # ----------------------
            # PAINTINGS AND PEOPLE LOCALIZATION
            # ----------------------
            people_room = self.__paintings_and_people_localization(paintings_detected)

            # If I want to execute only people localization, I will not show info
            # about paintings, otherwise I have `self.task == Task.all_in`
            # if self.task == Task.paintings_and_people_localization:
            #     paintings_detected = []

        # ------------------------------------------------------------------------

        # Step 18: Draw information about Paintings and People found
        # ----------------------------
        self.__draw_painting_and_people(
            img_original,
            paintings_detected,
            people_bounding_boxes,
            people_room,
            scale_factor
        )

        if self.task != Task.painting_rectification:
            # print("\n# Final frame shape: ", img_original.shape)
            self.show_image_main('pipeline_output_' + self.task.name, img_original, height=405, width=720)
            if self.media_type == MediaType.image:
                self.save_image(img_original)

        return img_original


import skimage.io as io
import skimage.transform as skt
import numpy as np
from PIL import Image
from src.models.class_patcher import patcher
from src.utils.imgproc import *

class patcher(patcher):
    def __init__(self, body='./body/body_aina.png', **options):
        super().__init__('愛奈', body=body, pantie_position=[0, 269], **options)
        self.mask = io.imread('./mask/mask_aina.png')

    def convert(self, image):
        pantie = np.array(image)
        pantie = np.bitwise_and(pantie, self.mask)

        front = pantie[:158, 1:200]
        back = pantie[:200, 460:]
        front = np.uint8(resize(front, [0.55, 0.57]) * 255)
        back = np.uint8(resize(back, [0.55, 0.57]) * 255)
        back = np.rot90(back, -1)

        out = np.zeros((115, 115, 4),  dtype=np.uint8)
        [fr, fc, _] = front.shape
        [br, bc, _] = back.shape
        out[:fr, :fc] = front
        out[-br:, -bc:] = out[-br:, -bc:] * (back[:, :, -1] <= 20)[:, :, None] + back * (back[:, :, -1] > 20)[:, :, None]
        return Image.fromarray(out)


import numpy as np
import argparse
import imutils
import sys
import cv2
from opts import parse_opts
from models.model import generate_model
import torch

def main():
    opts = parse_opts()
    if torch.cuda.is_available():
        opts.cuda = True
    opts.arch = '{}-{}'.format(opts.model, opts.model_depth)
    # load the human activity recognition model
    print("[INFO] loading human activity recognition model...")
    # net = cv2.dnn.readNet(args["model"])

    model, parameters = generate_model(opts)
    if opts.resume_path1:
        print('loading checkpoint {}'.format(opts.resume_path1))
        checkpoint = torch.load(opts.resume_path1)
        assert opts.arch == checkpoint['arch']
        model.load_state_dict(checkpoint['state_dict'])

    model.eval()

    # grab a pointer to the input video stream
    print("[INFO] accessing video stream...")
    vs = cv2.VideoCapture(opts.inputs)
    fourcc = cv2.VideoWriter_fourcc(*'XVID')
    v_writer = cv2.VideoWriter('mice.avi', fourcc, 10, (1280, 720))

    # loop until we explicitly break from it
    while True:
        # initialize the batch of frames that will be passed through the
        # model
        frames = []
        ori_frames = []
        # loop over the number of required sample frames
        for i in range(0, opts.sample_duration):
            # read a frame from the video stream
            (grabbed, frame) = vs.read()

            # if the frame was not grabbed then we've reached the end of
            # the video stream so exit the script
            if not grabbed:
                print("[INFO] no frame read from stream - exiting")
                sys.exit(0)
            ori_frame = frame.copy()
            ori_frames.append(ori_frame)

            # otherwise, the frame was read so resize it and add it to
            # our frames list
            frame = imutils.resize(frame, height=112)
            frames.append(frame)
        # now that our frames array is filled we can construct our blob
        blob = cv2.dnn.blobFromImages(frames, 1.0,
            (opts.sample_size, opts.sample_size), (114.7748, 107.7354, 99.4750),
            swapRB=True, crop=True)
        # blob = cv2.dnn.blobFromImages(frames, 1.0,
        #     (opts.sample_size, opts.sample_size), (0, 0, 0),
        #     swapRB=True, crop=True)

        # img = blob[0, :, :, :]
        # img = np.array(img, dtype=np.uint8)
        # img = np.transpose(img, (1, 2, 0))
        # cv2.imshow('img', img)
        # cv2.waitKey(0)
        # import pdb; pdb.set_trace()

        blob = np.transpose(blob, (1, 0, 2, 3))
        blob = np.expand_dims(blob, axis=0)

        # pass the blob through the network to obtain our human activity
        # recognition predictions
        inputs = torch.from_numpy(blob)
        outputs = model(inputs)
        label = np.array(torch.mean(outputs, dim=0, keepdim=True).topk(1)[1].cpu().data[0])
        print(label)
        if label[0] == 0:
            text = 'Normal'
        elif label[0] == 1:
            text = 'Ictal'
        else:
            text = 'NoneType'
        # loop over our frames
        for frame in ori_frames:
            # draw the predicted activity on the frame
            # cv2.rectangle(frame, (0, 0), (300, 40), (0, 0, 0), -1)
            cv2.putText(frame, text, (40, 40), cv2.FONT_HERSHEY_SIMPLEX,
                1, (255, 255, 255), 2)
            v_writer.write(frame)

            # display the frame to our screen
            cv2.imshow("Activity Recognition", frame)
            key = cv2.waitKey(1) & 0xFF

            # if the `q` key was pressed, break from the loop
            if key == ord("q"):
                break
    v_writer.release()

if __name__ == "__main__":
    main()


# -*- coding: utf-8 -*-
"""
Created on Thu Nov 22 23:00:57 2018

@author: Andy
"""
import time
import numpy as np
#Import functions as abbreviations
import robinhood_functions.robinhood_send_email as rhse
def market_analysis(trader,tick_notify,loss_trig,gain_trig,max_value):
    #Account position section analysis
    #Grab current securities, positions, and portfolio info
    securities = trader.securities_owned()
    watchlist = trader.watchlists()
    securities_tick = []
    watchlist_tick = []
    #Cyle though securities and grab quote date
    for itick, security in enumerate(securities['results']):
        #Request current stock info
        securities_tick.append(trader.security_tick(security['instrument']))
        quote_data = trader.quote_data(securities_tick[itick])
        last_trade_price = quote_data['last_trade_price']
        previous_close = quote_data['previous_close']
        average_buy_price = security['average_buy_price']
        quantity = security['quantity']
        if float(average_buy_price) == 0:
            average_buy_price = '1'
        #Does the library exist for this tick
        if securities_tick[itick] not in tick_notify:
            tick_notify[securities_tick[itick]] = 0
        #Send notification if it hasn't been sent yet
        if tick_notify[securities_tick[itick]] != 1:
            #Is the stock down by a certain percentage from average buy price
            if float(last_trade_price) < (float(previous_close)*(1-loss_trig)):
                #Number of shares to buy as per strategy
                buy_number_of_shares = np.floor(500/float(last_trade_price))
                #How much has this position lost
                loss_per = 100*(1-float(last_trade_price)/float(previous_close))
                #Message structured for three line text message
                msg0 = '%s @ %s From Avg %s\nDown %s%% From Prev Close\n Buy %s shares'\
                %(securities_tick[itick],last_trade_price[:5],average_buy_price[:5],
                str(loss_per)[:4],str(buy_number_of_shares))
                rhse.send_email(msg0,0)
                time.sleep(5)
                tick_notify[securities_tick[itick]] = 1
            #Is the stock up by a certain percantage from average buy price
#            elif float(last_trade_price) > (float(average_buy_price)*(1+gain_trig)):
#                #How much has this position gained
#                gain_per = 100*(float(last_trade_price)/float(average_buy_price)-1)
#                #Message structured for three line text message
#                msg0 = '%s, Trading @ %s From Avg %s\nUp: %s%%, Sell shares'\
#                %(securities_tick[itick],last_trade_price[:5],average_buy_price[:5],
#                str(gain_per)[:4])
#                rhse.send_email(msg0,0)
#                time.sleep(5)
#                tick_notify[securities_tick[itick]] = 1
            #Is the stock position above a max value
            elif (float(last_trade_price)*float(quantity)) > max_value:
                #Message structured for three line text message
                msg0 = '%s Over Max %s by %s\nSell shares @ %s'\
                %(securities_tick[itick],(str(max_value/1000)+'k')[:5],
                str(float(last_trade_price)*float(quantity)-max_value)[:6],last_trade_price[:5])
                rhse.send_email(msg0,0)
                time.sleep(5)
                tick_notify[securities_tick[itick]] = 1
    #Cyle though watchlist and grab ticks
    for itick, watch in enumerate(watchlist['results']):
        #Request current stock info
        tick_check = trader.security_tick(watch['instrument'])
        if tick_check not in securities_tick:
            watchlist_tick.append(tick_check)
    #Cyle though watchlist_tick and grab quote date
    for ticker in watchlist_tick:
        quote_data = trader.quote_data(ticker)
        last_trade_price = quote_data['last_trade_price']
        previous_close = quote_data['previous_close']
        #Does the library exist for this tick
        if ticker not in tick_notify:
            tick_notify[ticker] = 0
        #Send notification if it hasn't been sent yet
        if tick_notify[ticker] != 1:
            #Is the stock down by a certain percentage from previous close
            if float(last_trade_price) < (float(previous_close)*(1-loss_trig)):
                #How much has this position lost
                loss_per = 100*(1-float(last_trade_price)/float(previous_close))
                #Message structured for three line text message
                msg0 = '%s @ %s From Prev Close %s\n Down: %s%%'\
                %(ticker,last_trade_price[:5],previous_close[:5],
                  str(loss_per)[:4])
                rhse.send_email(msg0,0)
                time.sleep(5)
                tick_notify[ticker] = 1
    return None